From 10039f8131185e9b22d6f36c30ab1cbc34dd69f0 Mon Sep 17 00:00:00 2001
From: Francois Bleibel <fbleibel@gmail.com>
Date: Wed, 7 Jul 2010 19:40:46 +0900
Subject: [PATCH] Added code to sot-dynamic
---
AUTHORS | 1 +
COPYING | 15 +
ChangeLog | 1 +
INSTALL | 21 +
NEWS | 1 +
README | 1 +
.../CMakeDirectoryInformation.cmake | 21 +
.../documentation.dir/DependInfo.cmake | 8 +
doc/CMakeFiles/documentation.dir/build.make | 71 ++
.../documentation.dir/cmake_clean.cmake | 9 +
.../documentation.dir/progress.make | 2 +
doc/CMakeFiles/progress.marks | 1 +
doc/CMakeLists.txt | 32 +
doc/Makefile | 185 +++
doc/additionalDoc/package.hh | 8 +
doc/cmake_install.cmake | 46 +
doc/footer.html | 11 +
doc/header.html | 7 +
doc/package.css | 230 ++++
doc/package.dox | 295 +++++
doc/package.dox.cmake | 295 +++++
doc/pictures/footer.txt | 2 +
include/sot-dynamic/angle-estimator.h | 122 ++
include/sot-dynamic/dynamic-hrp2.h | 89 ++
include/sot-dynamic/dynamic.h | 233 ++++
include/sot-dynamic/force-compensation.h | 203 ++++
include/sot-dynamic/integrator-force-exact.h | 101 ++
include/sot-dynamic/integrator-force-rk4.h | 102 ++
include/sot-dynamic/integrator-force.h | 121 ++
include/sot-dynamic/mass-apparent.h | 97 ++
include/sot-dynamic/matrix-inertia.h | 84 ++
.../sot-dynamic/waist-attitude-from-sensor.h | 131 +++
include/sot-dynamic/zmpreffromcom.h | 102 ++
sot-dynamic.pc.cmake | 13 +
src/CMakeLists.txt | 130 +++
src/angle-estimator.cpp | 325 ++++++
src/dynamic-hrp2.cpp | 67 ++
src/dynamic.cpp | 1018 +++++++++++++++++
src/force-compensation.cpp | 638 +++++++++++
src/integrator-force-exact.cpp | 251 ++++
src/integrator-force-rk4.cpp | 136 +++
src/integrator-force.cpp | 171 +++
src/mass-apparent.cpp | 108 ++
src/matrix-inertia.cpp | 305 +++++
src/waist-attitude-from-sensor.cpp | 200 ++++
src/zmpreffromcom.cpp | 119 ++
unitTesting/CMakeLists.txt | 46 +
unitTesting/dummy.cpp | 8 +
48 files changed, 6183 insertions(+)
create mode 100644 AUTHORS
create mode 100644 COPYING
create mode 100644 ChangeLog
create mode 100644 INSTALL
create mode 100644 NEWS
create mode 100644 README
create mode 100644 doc/CMakeFiles/CMakeDirectoryInformation.cmake
create mode 100644 doc/CMakeFiles/documentation.dir/DependInfo.cmake
create mode 100644 doc/CMakeFiles/documentation.dir/build.make
create mode 100644 doc/CMakeFiles/documentation.dir/cmake_clean.cmake
create mode 100644 doc/CMakeFiles/documentation.dir/progress.make
create mode 100644 doc/CMakeFiles/progress.marks
create mode 100644 doc/CMakeLists.txt
create mode 100644 doc/Makefile
create mode 100644 doc/additionalDoc/package.hh
create mode 100644 doc/cmake_install.cmake
create mode 100644 doc/footer.html
create mode 100644 doc/header.html
create mode 100644 doc/package.css
create mode 100644 doc/package.dox
create mode 100644 doc/package.dox.cmake
create mode 100644 doc/pictures/footer.txt
create mode 100644 include/sot-dynamic/angle-estimator.h
create mode 100644 include/sot-dynamic/dynamic-hrp2.h
create mode 100644 include/sot-dynamic/dynamic.h
create mode 100644 include/sot-dynamic/force-compensation.h
create mode 100644 include/sot-dynamic/integrator-force-exact.h
create mode 100644 include/sot-dynamic/integrator-force-rk4.h
create mode 100644 include/sot-dynamic/integrator-force.h
create mode 100644 include/sot-dynamic/mass-apparent.h
create mode 100644 include/sot-dynamic/matrix-inertia.h
create mode 100644 include/sot-dynamic/waist-attitude-from-sensor.h
create mode 100644 include/sot-dynamic/zmpreffromcom.h
create mode 100644 sot-dynamic.pc.cmake
create mode 100644 src/CMakeLists.txt
create mode 100644 src/angle-estimator.cpp
create mode 100644 src/dynamic-hrp2.cpp
create mode 100644 src/dynamic.cpp
create mode 100644 src/force-compensation.cpp
create mode 100644 src/integrator-force-exact.cpp
create mode 100644 src/integrator-force-rk4.cpp
create mode 100644 src/integrator-force.cpp
create mode 100644 src/mass-apparent.cpp
create mode 100644 src/matrix-inertia.cpp
create mode 100644 src/waist-attitude-from-sensor.cpp
create mode 100644 src/zmpreffromcom.cpp
create mode 100644 unitTesting/CMakeLists.txt
create mode 100644 unitTesting/dummy.cpp
diff --git a/AUTHORS b/AUTHORS
new file mode 100644
index 0000000..77602e7
--- /dev/null
+++ b/AUTHORS
@@ -0,0 +1 @@
+Authors: Nicolas Mansard, Olivier Stasse, François Keith, François Bleibel
\ No newline at end of file
diff --git a/COPYING b/COPYING
new file mode 100644
index 0000000..453184a
--- /dev/null
+++ b/COPYING
@@ -0,0 +1,15 @@
+#
+# Copyright (c)
+#
+# Permission to use, copy, modify, and distribute this software for any
+# purpose with or without fee is hereby granted, provided that the above
+# copyright notice and this permission notice appear in all copies.
+#
+# THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+# WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+# MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+# ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+# WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+# ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+# OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+
diff --git a/ChangeLog b/ChangeLog
new file mode 100644
index 0000000..31f65ff
--- /dev/null
+++ b/ChangeLog
@@ -0,0 +1 @@
+Write in this file the modifications you commit.
diff --git a/INSTALL b/INSTALL
new file mode 100644
index 0000000..e95a6c4
--- /dev/null
+++ b/INSTALL
@@ -0,0 +1,21 @@
+#
+# Copyright
+#
+
+Installation instructions for library sot-dynamic
+----------------------------------------------
+
+It is recommended to create a specific directory to install this package.
+mkdir build
+cd build
+cmake [Options] ..
+make
+make install
+
+Options:
+
+-DCMAKE_INSTALL_PREFIX=...
+ specifies the directory where to install the package.
+
+-DCMAKE_BUILD_TYPE=[none|debug|release|relwithdebinfo|MinSizeRel]
+ specifies to type of compilation (release, debug, ...)
diff --git a/NEWS b/NEWS
new file mode 100644
index 0000000..1983664
--- /dev/null
+++ b/NEWS
@@ -0,0 +1 @@
+Write in this file the news related to package sot-dynamic.
diff --git a/README b/README
new file mode 100644
index 0000000..4818b74
--- /dev/null
+++ b/README
@@ -0,0 +1 @@
+See INSTALL for installation instruction.
diff --git a/doc/CMakeFiles/CMakeDirectoryInformation.cmake b/doc/CMakeFiles/CMakeDirectoryInformation.cmake
new file mode 100644
index 0000000..a75fe30
--- /dev/null
+++ b/doc/CMakeFiles/CMakeDirectoryInformation.cmake
@@ -0,0 +1,21 @@
+# CMAKE generated file: DO NOT EDIT!
+# Generated by "Unix Makefiles" Generator, CMake Version 2.8
+
+# Relative path conversion top directories.
+SET(CMAKE_RELATIVE_PATH_TOP_SOURCE "/home/blue/sot-devel/sot-dynamic")
+SET(CMAKE_RELATIVE_PATH_TOP_BINARY "/home/blue/sot-devel/sot-dynamic")
+
+# Force unix paths in dependencies.
+SET(CMAKE_FORCE_UNIX_PATHS 1)
+
+# The C and CXX include file search paths:
+SET(CMAKE_C_INCLUDE_PATH
+ )
+SET(CMAKE_CXX_INCLUDE_PATH ${CMAKE_C_INCLUDE_PATH})
+SET(CMAKE_Fortran_INCLUDE_PATH ${CMAKE_C_INCLUDE_PATH})
+
+# The C and CXX include file regular expressions for this directory.
+SET(CMAKE_C_INCLUDE_REGEX_SCAN "^.*$")
+SET(CMAKE_C_INCLUDE_REGEX_COMPLAIN "^$")
+SET(CMAKE_CXX_INCLUDE_REGEX_SCAN ${CMAKE_C_INCLUDE_REGEX_SCAN})
+SET(CMAKE_CXX_INCLUDE_REGEX_COMPLAIN ${CMAKE_C_INCLUDE_REGEX_COMPLAIN})
diff --git a/doc/CMakeFiles/documentation.dir/DependInfo.cmake b/doc/CMakeFiles/documentation.dir/DependInfo.cmake
new file mode 100644
index 0000000..ac621c5
--- /dev/null
+++ b/doc/CMakeFiles/documentation.dir/DependInfo.cmake
@@ -0,0 +1,8 @@
+# The set of languages for which implicit dependencies are needed:
+SET(CMAKE_DEPENDS_LANGUAGES
+ )
+# The set of files for implicit dependencies of each language:
+
+# Targets to which this target links.
+SET(CMAKE_TARGET_LINKED_INFO_FILES
+ )
diff --git a/doc/CMakeFiles/documentation.dir/build.make b/doc/CMakeFiles/documentation.dir/build.make
new file mode 100644
index 0000000..f2c20a7
--- /dev/null
+++ b/doc/CMakeFiles/documentation.dir/build.make
@@ -0,0 +1,71 @@
+# CMAKE generated file: DO NOT EDIT!
+# Generated by "Unix Makefiles" Generator, CMake Version 2.8
+
+#=============================================================================
+# Special targets provided by cmake.
+
+# Disable implicit rules so canoncical targets will work.
+.SUFFIXES:
+
+# Remove some rules from gmake that .SUFFIXES does not remove.
+SUFFIXES =
+
+.SUFFIXES: .hpux_make_needs_suffix_list
+
+# Produce verbose output by default.
+VERBOSE = 1
+
+# Suppress display of executed commands.
+$(VERBOSE).SILENT:
+
+# A target that is always out of date.
+cmake_force:
+.PHONY : cmake_force
+
+#=============================================================================
+# Set environment variables for the build.
+
+# The shell in which to execute make rules.
+SHELL = /bin/sh
+
+# The CMake executable.
+CMAKE_COMMAND = /usr/bin/cmake
+
+# The command to remove a file.
+RM = /usr/bin/cmake -E remove -f
+
+# The program to use to edit the cache.
+CMAKE_EDIT_COMMAND = /usr/bin/ccmake
+
+# The top-level source directory on which CMake was run.
+CMAKE_SOURCE_DIR = /home/blue/sot-devel/sot-dynamic
+
+# The top-level build directory on which CMake was run.
+CMAKE_BINARY_DIR = /home/blue/sot-devel/sot-dynamic
+
+# Utility rule file for documentation.
+
+doc/CMakeFiles/documentation: doc/html/index.html
+
+doc/html/index.html:
+ $(CMAKE_COMMAND) -E cmake_progress_report /home/blue/sot-devel/sot-dynamic/CMakeFiles $(CMAKE_PROGRESS_1)
+ @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --blue --bold "Generating html/index.html"
+ cd /home/blue/sot-devel/sot-dynamic/doc && /usr/bin/doxygen "/home/blue/sot-devel/sot-dynamic/doc/package.dox"
+
+documentation: doc/CMakeFiles/documentation
+documentation: doc/html/index.html
+documentation: doc/CMakeFiles/documentation.dir/build.make
+.PHONY : documentation
+
+# Rule to build all files generated by this target.
+doc/CMakeFiles/documentation.dir/build: documentation
+.PHONY : doc/CMakeFiles/documentation.dir/build
+
+doc/CMakeFiles/documentation.dir/clean:
+ cd /home/blue/sot-devel/sot-dynamic/doc && $(CMAKE_COMMAND) -P CMakeFiles/documentation.dir/cmake_clean.cmake
+.PHONY : doc/CMakeFiles/documentation.dir/clean
+
+doc/CMakeFiles/documentation.dir/depend:
+ cd /home/blue/sot-devel/sot-dynamic && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/blue/sot-devel/sot-dynamic /home/blue/sot-devel/sot-dynamic/doc /home/blue/sot-devel/sot-dynamic /home/blue/sot-devel/sot-dynamic/doc /home/blue/sot-devel/sot-dynamic/doc/CMakeFiles/documentation.dir/DependInfo.cmake --color=$(COLOR)
+.PHONY : doc/CMakeFiles/documentation.dir/depend
+
diff --git a/doc/CMakeFiles/documentation.dir/cmake_clean.cmake b/doc/CMakeFiles/documentation.dir/cmake_clean.cmake
new file mode 100644
index 0000000..8703e7a
--- /dev/null
+++ b/doc/CMakeFiles/documentation.dir/cmake_clean.cmake
@@ -0,0 +1,9 @@
+FILE(REMOVE_RECURSE
+ "CMakeFiles/documentation"
+ "html/index.html"
+)
+
+# Per-language clean rules from dependency scanning.
+FOREACH(lang)
+ INCLUDE(CMakeFiles/documentation.dir/cmake_clean_${lang}.cmake OPTIONAL)
+ENDFOREACH(lang)
diff --git a/doc/CMakeFiles/documentation.dir/progress.make b/doc/CMakeFiles/documentation.dir/progress.make
new file mode 100644
index 0000000..164e1d2
--- /dev/null
+++ b/doc/CMakeFiles/documentation.dir/progress.make
@@ -0,0 +1,2 @@
+CMAKE_PROGRESS_1 = 2
+
diff --git a/doc/CMakeFiles/progress.marks b/doc/CMakeFiles/progress.marks
new file mode 100644
index 0000000..d00491f
--- /dev/null
+++ b/doc/CMakeFiles/progress.marks
@@ -0,0 +1 @@
+1
diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt
new file mode 100644
index 0000000..2b6030a
--- /dev/null
+++ b/doc/CMakeLists.txt
@@ -0,0 +1,32 @@
+#
+# Copyright
+#
+
+# Configure package.dox file
+CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/package.dox.cmake
+ ${CMAKE_CURRENT_BINARY_DIR}/package.dox
+)
+
+
+ADD_CUSTOM_COMMAND(OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/html/index.html
+ COMMAND ${DOXYGEN_EXECUTABLE} \"${CMAKE_CURRENT_BINARY_DIR}/package.dox\"
+)
+
+ADD_CUSTOM_TARGET(documentation ALL
+ DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/html/index.html)
+
+#
+# Install html documentation
+#
+
+# html files
+INSTALL(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/html
+ DESTINATION ${CMAKE_INSTALL_PREFIX}/share/doc/${PROJECT_NAME})
+
+# doxytag
+INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}.doxytag
+ DESTINATION ${CMAKE_INSTALL_PREFIX}/share/doc/${PROJECT_NAME}/html)
+
+# pictures
+INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/pictures
+ DESTINATION ${CMAKE_INSTALL_PREFIX}/share/doc/${PROJECT_NAME}/html)
diff --git a/doc/Makefile b/doc/Makefile
new file mode 100644
index 0000000..d113b06
--- /dev/null
+++ b/doc/Makefile
@@ -0,0 +1,185 @@
+# CMAKE generated file: DO NOT EDIT!
+# Generated by "Unix Makefiles" Generator, CMake Version 2.8
+
+# Default target executed when no arguments are given to make.
+default_target: all
+.PHONY : default_target
+
+#=============================================================================
+# Special targets provided by cmake.
+
+# Disable implicit rules so canoncical targets will work.
+.SUFFIXES:
+
+# Remove some rules from gmake that .SUFFIXES does not remove.
+SUFFIXES =
+
+.SUFFIXES: .hpux_make_needs_suffix_list
+
+# Produce verbose output by default.
+VERBOSE = 1
+
+# Suppress display of executed commands.
+$(VERBOSE).SILENT:
+
+# A target that is always out of date.
+cmake_force:
+.PHONY : cmake_force
+
+#=============================================================================
+# Set environment variables for the build.
+
+# The shell in which to execute make rules.
+SHELL = /bin/sh
+
+# The CMake executable.
+CMAKE_COMMAND = /usr/bin/cmake
+
+# The command to remove a file.
+RM = /usr/bin/cmake -E remove -f
+
+# The program to use to edit the cache.
+CMAKE_EDIT_COMMAND = /usr/bin/ccmake
+
+# The top-level source directory on which CMake was run.
+CMAKE_SOURCE_DIR = /home/blue/sot-devel/sot-dynamic
+
+# The top-level build directory on which CMake was run.
+CMAKE_BINARY_DIR = /home/blue/sot-devel/sot-dynamic
+
+#=============================================================================
+# Targets provided globally by CMake.
+
+# Special rule for the target edit_cache
+edit_cache:
+ @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake cache editor..."
+ /usr/bin/ccmake -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR)
+.PHONY : edit_cache
+
+# Special rule for the target edit_cache
+edit_cache/fast: edit_cache
+.PHONY : edit_cache/fast
+
+# Special rule for the target install
+install: preinstall
+ @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..."
+ /usr/bin/cmake -P cmake_install.cmake
+.PHONY : install
+
+# Special rule for the target install
+install/fast: preinstall/fast
+ @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Install the project..."
+ /usr/bin/cmake -P cmake_install.cmake
+.PHONY : install/fast
+
+# Special rule for the target install/local
+install/local: preinstall
+ @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing only the local directory..."
+ /usr/bin/cmake -DCMAKE_INSTALL_LOCAL_ONLY=1 -P cmake_install.cmake
+.PHONY : install/local
+
+# Special rule for the target install/local
+install/local/fast: install/local
+.PHONY : install/local/fast
+
+# Special rule for the target install/strip
+install/strip: preinstall
+ @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Installing the project stripped..."
+ /usr/bin/cmake -DCMAKE_INSTALL_DO_STRIP=1 -P cmake_install.cmake
+.PHONY : install/strip
+
+# Special rule for the target install/strip
+install/strip/fast: install/strip
+.PHONY : install/strip/fast
+
+# Special rule for the target list_install_components
+list_install_components:
+ @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Available install components are: \"Unspecified\""
+.PHONY : list_install_components
+
+# Special rule for the target list_install_components
+list_install_components/fast: list_install_components
+.PHONY : list_install_components/fast
+
+# Special rule for the target rebuild_cache
+rebuild_cache:
+ @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..."
+ /usr/bin/cmake -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR)
+.PHONY : rebuild_cache
+
+# Special rule for the target rebuild_cache
+rebuild_cache/fast: rebuild_cache
+.PHONY : rebuild_cache/fast
+
+# The main all target
+all: cmake_check_build_system
+ cd /home/blue/sot-devel/sot-dynamic && $(CMAKE_COMMAND) -E cmake_progress_start /home/blue/sot-devel/sot-dynamic/CMakeFiles /home/blue/sot-devel/sot-dynamic/doc/CMakeFiles/progress.marks
+ cd /home/blue/sot-devel/sot-dynamic && $(MAKE) -f CMakeFiles/Makefile2 doc/all
+ $(CMAKE_COMMAND) -E cmake_progress_start /home/blue/sot-devel/sot-dynamic/CMakeFiles 0
+.PHONY : all
+
+# The main clean target
+clean:
+ cd /home/blue/sot-devel/sot-dynamic && $(MAKE) -f CMakeFiles/Makefile2 doc/clean
+.PHONY : clean
+
+# The main clean target
+clean/fast: clean
+.PHONY : clean/fast
+
+# Prepare targets for installation.
+preinstall: all
+ cd /home/blue/sot-devel/sot-dynamic && $(MAKE) -f CMakeFiles/Makefile2 doc/preinstall
+.PHONY : preinstall
+
+# Prepare targets for installation.
+preinstall/fast:
+ cd /home/blue/sot-devel/sot-dynamic && $(MAKE) -f CMakeFiles/Makefile2 doc/preinstall
+.PHONY : preinstall/fast
+
+# clear depends
+depend:
+ cd /home/blue/sot-devel/sot-dynamic && $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1
+.PHONY : depend
+
+# Convenience name for target.
+doc/CMakeFiles/documentation.dir/rule:
+ cd /home/blue/sot-devel/sot-dynamic && $(MAKE) -f CMakeFiles/Makefile2 doc/CMakeFiles/documentation.dir/rule
+.PHONY : doc/CMakeFiles/documentation.dir/rule
+
+# Convenience name for target.
+documentation: doc/CMakeFiles/documentation.dir/rule
+.PHONY : documentation
+
+# fast build rule for target.
+documentation/fast:
+ cd /home/blue/sot-devel/sot-dynamic && $(MAKE) -f doc/CMakeFiles/documentation.dir/build.make doc/CMakeFiles/documentation.dir/build
+.PHONY : documentation/fast
+
+# Help Target
+help:
+ @echo "The following are some of the valid targets for this Makefile:"
+ @echo "... all (the default if no target is provided)"
+ @echo "... clean"
+ @echo "... depend"
+ @echo "... documentation"
+ @echo "... edit_cache"
+ @echo "... install"
+ @echo "... install/local"
+ @echo "... install/strip"
+ @echo "... list_install_components"
+ @echo "... rebuild_cache"
+.PHONY : help
+
+
+
+#=============================================================================
+# Special targets to cleanup operation of make.
+
+# Special rule to run CMake to check the build system integrity.
+# No rule that depends on this can have commands that come from listfiles
+# because they might be regenerated.
+cmake_check_build_system:
+ cd /home/blue/sot-devel/sot-dynamic && $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0
+.PHONY : cmake_check_build_system
+
diff --git a/doc/additionalDoc/package.hh b/doc/additionalDoc/package.hh
new file mode 100644
index 0000000..6a10da3
--- /dev/null
+++ b/doc/additionalDoc/package.hh
@@ -0,0 +1,8 @@
+/**
+\mainpage
+
+\section intro_sot-dynamic Introduction
+
+This package implements ...
+
+*/
diff --git a/doc/cmake_install.cmake b/doc/cmake_install.cmake
new file mode 100644
index 0000000..902679b
--- /dev/null
+++ b/doc/cmake_install.cmake
@@ -0,0 +1,46 @@
+# Install script for directory: /home/blue/sot-devel/sot-dynamic/doc
+
+# Set the install prefix
+IF(NOT DEFINED CMAKE_INSTALL_PREFIX)
+ SET(CMAKE_INSTALL_PREFIX "/home/blue/sot-lib")
+ENDIF(NOT DEFINED CMAKE_INSTALL_PREFIX)
+STRING(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}")
+
+# Set the install configuration name.
+IF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME)
+ IF(BUILD_TYPE)
+ STRING(REGEX REPLACE "^[^A-Za-z0-9_]+" ""
+ CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}")
+ ELSE(BUILD_TYPE)
+ SET(CMAKE_INSTALL_CONFIG_NAME "DEBUG")
+ ENDIF(BUILD_TYPE)
+ MESSAGE(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"")
+ENDIF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME)
+
+# Set the component getting installed.
+IF(NOT CMAKE_INSTALL_COMPONENT)
+ IF(COMPONENT)
+ MESSAGE(STATUS "Install component: \"${COMPONENT}\"")
+ SET(CMAKE_INSTALL_COMPONENT "${COMPONENT}")
+ ELSE(COMPONENT)
+ SET(CMAKE_INSTALL_COMPONENT)
+ ENDIF(COMPONENT)
+ENDIF(NOT CMAKE_INSTALL_COMPONENT)
+
+# Install shared libraries without execute permission?
+IF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE)
+ SET(CMAKE_INSTALL_SO_NO_EXE "1")
+ENDIF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE)
+
+IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
+ FILE(INSTALL DESTINATION "/home/blue/sot-lib/share/doc/sot-dynamic" TYPE DIRECTORY FILES "/home/blue/sot-devel/sot-dynamic/doc/html")
+ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
+
+IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
+ FILE(INSTALL DESTINATION "/home/blue/sot-lib/share/doc/sot-dynamic/html" TYPE FILE FILES "/home/blue/sot-devel/sot-dynamic/doc/sot-dynamic.doxytag")
+ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
+
+IF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
+ FILE(INSTALL DESTINATION "/home/blue/sot-lib/share/doc/sot-dynamic/html" TYPE DIRECTORY FILES "/home/blue/sot-devel/sot-dynamic/doc/pictures")
+ENDIF(NOT CMAKE_INSTALL_COMPONENT OR "${CMAKE_INSTALL_COMPONENT}" STREQUAL "Unspecified")
+
diff --git a/doc/footer.html b/doc/footer.html
new file mode 100644
index 0000000..de1c8e1
--- /dev/null
+++ b/doc/footer.html
@@ -0,0 +1,11 @@
+ <br><br>
+ <hr>
+ <center>
+ <img src="./pictures/footer.jpg" Height=100>
+ <br>sot-dynamic library documentation</br>
+ </center>
+ <hr>
+ </center>
+ </body>
+ </head>
+
diff --git a/doc/header.html b/doc/header.html
new file mode 100644
index 0000000..537722c
--- /dev/null
+++ b/doc/header.html
@@ -0,0 +1,7 @@
+<HTML>
+ <HEAD>
+ <TITLE>sot-dynamic library documentation</TITLE>
+ <LINK HREF="package.css" REL="stylesheet" TYPE="text/css">
+ </HEAD>
+ <BODY>
+
diff --git a/doc/package.css b/doc/package.css
new file mode 100644
index 0000000..f992392
--- /dev/null
+++ b/doc/package.css
@@ -0,0 +1,230 @@
+body {
+ font-family: 'Lucida Grande','Lucida Sans Unicode',Verdana,Sans-Serif;
+ color: #5D5D5D;
+}
+
+dl {
+ border: 1.5px #82b6d7 solid;
+ width: 97%;
+ padding: 5px;
+ color: #330077;
+}
+
+code {
+ color: #3C9A35;
+}
+
+td.md {
+ color: #0066CC;
+}
+
+h1 {
+ padding-top: 50px;
+ padding: 0px;
+ font-family: 'Lucida Grande','Lucida Sans Unicode',Verdana,Sans-Serif;
+ font-variant: small-caps;
+ color:#0066CC;
+ text-align: center;
+}
+
+h2,h3,hr {
+ magin-top: 15px;
+ padding: 0px;
+ font-family: 'Lucida Grande','Lucida Sans Unicode',Verdana,Sans-Serif;
+ font-variant: small-caps;
+ color:#0066CC;
+}
+
+h4 {
+ color: #3C9A35;
+}
+
+a:link {
+ font-weight: bold;
+ text-decoration: none;
+ color:#0066CC
+}
+
+a:hover, a:active {
+ text-decoration: underline;
+ color: #3C9A35;
+}
+
+a:visited {
+ font-weight: bold;
+ color: #3C9A35;
+ text-decoration: none;
+}
+
+DIV.memitem
+{
+ spacing: 10px;
+ width:100%;
+ background:#FFFFFF;
+ font-size:100%;
+ line-height:normal;
+ border-width: 1px;
+ border-style: solid;
+ border-color: #808080;
+ -moz-border-radius: 8px 8px 8px 8px;
+}
+
+DIV.memproto
+{
+ width:100%;
+ background:#F0F0F0;
+ font-size:100%;
+ line-height:normal;
+ border-width: 1px;
+ border-style: solid;
+ border-color: #808080;
+ -moz-border-radius: 8px 8px 8px 8px;
+}
+
+DIV.memdoc
+{
+ padding: 10px;
+ width:100%;
+ font-size:100%;
+ line-height:normal;
+}
+
+DIV.tabs
+{
+ float : left;
+ width : 100%;
+ background : url("tab_b.gif") repeat-x bottom;
+ margin-bottom : 4px;
+}
+
+DIV.tabs UL
+{
+ margin : 0px;
+ padding-left : 10px;
+ list-style : none;
+}
+
+DIV.tabs LI, DIV.tabs FORM
+{
+ display : inline;
+ margin : 0px;
+ padding : 0px;
+}
+
+DIV.tabs FORM
+{
+ float : right;
+}
+
+DIV.tabs A
+{
+ float : left;
+ background : url("tab_r.gif") no-repeat right top;
+ border-bottom : 1px solid #84B0C7;
+ font-size : x-small;
+ font-weight : bold;
+ text-decoration : none;
+}
+
+DIV.tabs A:hover
+{
+ background-position: 100% -150px;
+}
+
+DIV.tabs A:link, DIV.tabs A:visited,
+DIV.tabs A:active, DIV.tabs A:hover
+{
+ color: #1A419D;
+}
+
+DIV.tabs SPAN
+{
+ float : left;
+ display : block;
+ background : url("tab_l.gif") no-repeat left top;
+ padding : 5px 9px;
+ white-space : nowrap;
+}
+
+DIV.tabs INPUT
+{
+ float : right;
+ display : inline;
+ font-size : 1em;
+}
+
+DIV.tabs TD
+{
+ font-size : x-small;
+ font-weight : bold;
+ text-decoration : none;
+}
+
+
+
+DIV.tabs SPAN {float : none;}
+
+DIV.tabs A:hover SPAN
+{
+ background-position: 0% -150px;
+}
+
+DIV.tabs LI#current A
+{
+ background-position: 100% -150px;
+ border-width : 0px;
+}
+
+DIV.tabs LI#current SPAN
+{
+ background-position: 0% -150px;
+ padding-bottom : 6px;
+}
+
+DIV.nav
+{
+ background : none;
+ border : none;
+ border-bottom : 1px solid #84B0C7;
+}
+
+DIV.groupHeader
+{
+ padding-top: 30px;
+ padding-bottom: 20px;
+ background : none;
+ border : none;
+ border-bottom : 1px solid #84B0C7;
+ font-family: 'Lucida Grande','Lucida Sans Unicode',Verdana,Sans-Serif;
+ font-variant: small-caps;
+ font-size: 14pt;
+ color:#0066CC;
+}
+
+.directory p
+{
+ margin: 0px;
+ white-space: nowrap;
+ font-family: 'Lucida Grande','Lucida Sans Unicode',Verdana,Sans-Serif;
+ font-size: 10pt;
+ font-weight: normal;
+}
+
+
+.directory h3
+{
+ font-family: 'Lucida Grande','Lucida Sans Unicode',Verdana,Sans-Serif;
+ margin: 0px;
+ margin-top: 1em;
+ padding-bottom: 20px;
+ font-size: 12pt;
+ font-variant: small-caps;
+ text-align: center;
+}
+
+.directory a:visited {
+ font-weight: bold;
+ text-decoration: none;
+ color:#0066CC
+}
+
diff --git a/doc/package.dox b/doc/package.dox
new file mode 100644
index 0000000..8e7bd1b
--- /dev/null
+++ b/doc/package.dox
@@ -0,0 +1,295 @@
+# Doxyfile 1.4.2
+
+
+#---------------------------------------------------------------------------
+# Project related configuration options
+#---------------------------------------------------------------------------
+OUTPUT_DIRECTORY = .
+CREATE_SUBDIRS = NO
+OUTPUT_LANGUAGE = English
+BRIEF_MEMBER_DESC = YES
+REPEAT_BRIEF = YES
+ABBREVIATE_BRIEF = "The $name class" \
+ "The $name widget" \
+ "The $name file" \
+ is \
+ provides \
+ specifies \
+ contains \
+ represents \
+ a \
+ an \
+ the
+ALWAYS_DETAILED_SEC = YES
+INLINE_INHERITED_MEMB = NO
+FULL_PATH_NAMES = NO
+STRIP_FROM_PATH =
+STRIP_FROM_INC_PATH =
+SHORT_NAMES = NO
+JAVADOC_AUTOBRIEF = YES
+MULTILINE_CPP_IS_BRIEF = NO
+DETAILS_AT_TOP = YES
+INHERIT_DOCS = YES
+DISTRIBUTE_GROUP_DOC = NO
+SEPARATE_MEMBER_PAGES = NO
+TAB_SIZE = 2
+OPTIMIZE_OUTPUT_FOR_C = NO
+OPTIMIZE_OUTPUT_JAVA = NO
+SUBGROUPING = YES
+
+#---------------------------------------------------------------------------
+# Build related configuration options
+#---------------------------------------------------------------------------
+EXTRACT_LOCAL_CLASSES = YES
+EXTRACT_LOCAL_METHODS = NO
+CASE_SENSE_NAMES = NO
+HIDE_SCOPE_NAMES = NO
+SHOW_INCLUDE_FILES = NO
+INLINE_INFO = YES
+SORT_MEMBER_DOCS = YES
+SORT_BRIEF_DOCS = NO
+SORT_BY_SCOPE_NAME = NO
+MAX_INITIALIZER_LINES = 30
+FILE_VERSION_FILTER =
+#---------------------------------------------------------------------------
+# configuration options related to warning and progress messages
+#---------------------------------------------------------------------------
+QUIET = NO
+WARNINGS = YES
+WARN_IF_UNDOCUMENTED = YES
+WARN_IF_DOC_ERROR = YES
+WARN_NO_PARAMDOC = NO
+WARN_FORMAT = "$file:$line: $text"
+WARN_LOGFILE =
+#---------------------------------------------------------------------------
+# configuration options related to the input files
+#---------------------------------------------------------------------------
+RECURSIVE = YES
+EXCLUDE =
+EXCLUDE_SYMLINKS = NO
+EXAMPLE_PATH =
+EXAMPLE_PATTERNS = *
+EXAMPLE_RECURSIVE = NO
+INPUT_FILTER =
+FILTER_PATTERNS =
+FILTER_SOURCE_FILES = NO
+#---------------------------------------------------------------------------
+# configuration options related to source browsing
+#---------------------------------------------------------------------------
+#---------------------------------------------------------------------------
+# configuration options related to the alphabetical class index
+#---------------------------------------------------------------------------
+ALPHABETICAL_INDEX = NO
+COLS_IN_ALPHA_INDEX = 5
+IGNORE_PREFIX =
+#---------------------------------------------------------------------------
+# configuration options related to the HTML output
+#---------------------------------------------------------------------------
+GENERATE_HTML = YES
+HTML_FILE_EXTENSION = .html
+HTML_ALIGN_MEMBERS = YES
+GENERATE_HTMLHELP = NO
+CHM_FILE =
+HHC_LOCATION =
+GENERATE_CHI = NO
+BINARY_TOC = NO
+TOC_EXPAND = YES
+DISABLE_INDEX = NO
+ENUM_VALUES_PER_LINE = 1
+GENERATE_TREEVIEW = YES
+TREEVIEW_WIDTH = 250
+#---------------------------------------------------------------------------
+# configuration options related to the LaTeX output
+#---------------------------------------------------------------------------
+LATEX_OUTPUT = latex
+LATEX_CMD_NAME = latex
+MAKEINDEX_CMD_NAME = makeindex
+COMPACT_LATEX = NO
+PAPER_TYPE = a4wide
+EXTRA_PACKAGES =
+LATEX_HEADER =
+PDF_HYPERLINKS = NO
+USE_PDFLATEX = NO
+LATEX_BATCHMODE = NO
+LATEX_HIDE_INDICES = NO
+#---------------------------------------------------------------------------
+# configuration options related to the RTF output
+#---------------------------------------------------------------------------
+RTF_OUTPUT = rtf
+COMPACT_RTF = NO
+RTF_HYPERLINKS = NO
+RTF_STYLESHEET_FILE =
+RTF_EXTENSIONS_FILE =
+#---------------------------------------------------------------------------
+# configuration options related to the man page output
+#---------------------------------------------------------------------------
+MAN_OUTPUT = man
+MAN_EXTENSION = .3
+MAN_LINKS = NO
+#---------------------------------------------------------------------------
+# configuration options related to the XML output
+#---------------------------------------------------------------------------
+XML_OUTPUT = xml
+XML_SCHEMA =
+XML_DTD =
+XML_PROGRAMLISTING = YES
+#---------------------------------------------------------------------------
+# configuration options for the AutoGen Definitions output
+#---------------------------------------------------------------------------
+GENERATE_AUTOGEN_DEF = NO
+#---------------------------------------------------------------------------
+# configuration options related to the Perl module output
+#---------------------------------------------------------------------------
+PERLMOD_LATEX = NO
+PERLMOD_PRETTY = YES
+PERLMOD_MAKEVAR_PREFIX =
+#---------------------------------------------------------------------------
+# Configuration options related to the preprocessor
+#---------------------------------------------------------------------------
+ENABLE_PREPROCESSING = YES
+MACRO_EXPANSION = YES
+EXPAND_ONLY_PREDEF = YES
+SEARCH_INCLUDES = YES
+INCLUDE_PATH =
+INCLUDE_FILE_PATTERNS =
+EXPAND_AS_DEFINED =
+SKIP_FUNCTION_MACROS = YES
+#---------------------------------------------------------------------------
+# Configuration::additions related to external references
+#---------------------------------------------------------------------------
+TAGFILES = \
+ /home/blue/sot-lib/share/doc/dynamicsJRLJapan/html/dynamicsJRLJapan.doxytag=/home/blue/sot-lib/share/doc/dynamicsJRLJapan/html \
+ /home/blue/sot-lib/share/doc/hrp2Dynamics/html/hrp2Dynamics.doxytag=/home/blue/sot-lib/share/doc/hrp2Dynamics/html \
+ /home/blue/sot-lib/share/doc/dynamic-graph/html/dynamic-graph.doxytag=/home/blue/sot-lib/share/doc/dynamic-graph/html \
+ /home/blue/sot-lib/share/doc/sot-core/html/sot-core.doxytag=/home/blue/sot-lib/share/doc/sot-core/html \
+ /home/blue/sot-lib/share/doc/MatrixAbstractLayer/html/MatrixAbstractLayer.doxytag=/home/blue/sot-lib/share/doc/MatrixAbstractLayer/html
+GENERATE_TAGFILE = sot-dynamic.doxytag
+ALLEXTERNALS = NO
+EXTERNAL_GROUPS = YES
+PERL_PATH = /usr/bin/perl
+#---------------------------------------------------------------------------
+# Configuration options related to the dot tool
+#---------------------------------------------------------------------------
+#---------------------------------------------------------------------------
+# Configuration::additions related to the search engine
+#---------------------------------------------------------------------------
+SEARCHENGINE = NO
+
+
+
+
+#---------------------------------------------------------------------------
+# Project related configuration options
+#---------------------------------------------------------------------------
+PROJECT_NAME = "sot-dynamic library documentation"
+PROJECT_NUMBER = 1.0
+
+#---------------------------------------------------------------------------
+# Build related configuration options
+#---------------------------------------------------------------------------
+EXTRACT_ALL = YES
+EXTRACT_PRIVATE = YES
+EXTRACT_STATIC = NO
+HIDE_UNDOC_MEMBERS = YES
+HIDE_UNDOC_CLASSES = YES
+HIDE_FRIEND_COMPOUNDS = YES
+HIDE_IN_BODY_DOCS = NO
+INTERNAL_DOCS = NO
+SHOW_INCLUDE_FILES = NO
+GENERATE_TODOLIST = YES
+GENERATE_TESTLIST = NO
+GENERATE_BUGLIST = NO
+GENERATE_DEPRECATEDLIST= YES
+ENABLED_SECTIONS =
+SHOW_USED_FILES = NO
+SHOW_DIRECTORIES = NO
+#---------------------------------------------------------------------------
+# configuration options related to warning and progress messages
+#---------------------------------------------------------------------------
+#---------------------------------------------------------------------------
+# configuration options related to the input files
+#---------------------------------------------------------------------------
+FILE_PATTERNS = *.hh *.idl
+EXCLUDE_PATTERNS =
+INPUT = /home/blue/sot-devel/sot-dynamic/include \
+ /home/blue/sot-devel/sot-dynamic/doc/additionalDoc
+
+#---------------------------------------------------------------------------
+# configuration options related to inserting images
+#---------------------------------------------------------------------------
+IMAGE_PATH = /home/blue/sot-devel/sot-dynamic/doc/figures
+#---------------------------------------------------------------------------
+# configuration options related to source browsing
+#---------------------------------------------------------------------------
+SOURCE_BROWSER = NO
+INLINE_SOURCES = NO
+STRIP_CODE_COMMENTS = YES
+REFERENCED_BY_RELATION = NO
+REFERENCES_RELATION = NO
+VERBATIM_HEADERS = NO
+#---------------------------------------------------------------------------
+# configuration options related to the alphabetical class index
+#---------------------------------------------------------------------------
+#---------------------------------------------------------------------------
+# configuration options related to the HTML output
+#---------------------------------------------------------------------------
+HTML_OUTPUT = html
+HTML_STYLESHEET = /home/blue/sot-devel/sot-dynamic/doc/package.css
+HTML_HEADER = /home/blue/sot-devel/sot-dynamic/doc/header.html
+HTML_FOOTER = /home/blue/sot-devel/sot-dynamic/doc/footer.html
+
+#---------------------------------------------------------------------------
+# configuration options related to the LaTeX output
+#---------------------------------------------------------------------------
+GENERATE_LATEX = NO
+USE_PDFLATEX = YES
+#---------------------------------------------------------------------------
+# configuration options related to the RTF output
+#---------------------------------------------------------------------------
+GENERATE_RTF = NO
+#---------------------------------------------------------------------------
+# configuration options related to the man page output
+#---------------------------------------------------------------------------
+GENERATE_MAN = NO
+#---------------------------------------------------------------------------
+# configuration options related to the XML output
+#---------------------------------------------------------------------------
+GENERATE_XML = NO
+#---------------------------------------------------------------------------
+# configuration options for the AutoGen Definitions output
+#---------------------------------------------------------------------------
+GENERATE_AUTOGEN_DEF = NO
+#---------------------------------------------------------------------------
+# configuration options related to the Perl module output
+#---------------------------------------------------------------------------
+GENERATE_PERLMOD = NO
+#---------------------------------------------------------------------------
+# Configuration options related to the preprocessor
+#---------------------------------------------------------------------------
+#---------------------------------------------------------------------------
+# Configuration::additions related to external references
+#---------------------------------------------------------------------------
+#---------------------------------------------------------------------------
+# Configuration options related to the dot tool
+#---------------------------------------------------------------------------
+CLASS_DIAGRAMS = YES
+HIDE_UNDOC_RELATIONS = YES
+HAVE_DOT = YES
+CLASS_GRAPH = NO
+COLLABORATION_GRAPH = YES
+GROUP_GRAPHS = YES
+UML_LOOK = NO
+TEMPLATE_RELATIONS = NO
+INCLUDE_GRAPH = YES
+INCLUDED_BY_GRAPH = YES
+CALL_GRAPH = NO
+GRAPHICAL_HIERARCHY = YES
+DIRECTORY_GRAPH = YES
+DOT_IMAGE_FORMAT = png
+DOT_PATH =
+DOTFILE_DIRS =
+MAX_DOT_GRAPH_DEPTH = 1000
+DOT_TRANSPARENT = NO
+DOT_MULTI_TARGETS = YES
+GENERATE_LEGEND = YES
+DOT_CLEANUP = YES
diff --git a/doc/package.dox.cmake b/doc/package.dox.cmake
new file mode 100644
index 0000000..1921477
--- /dev/null
+++ b/doc/package.dox.cmake
@@ -0,0 +1,295 @@
+# Doxyfile 1.4.2
+
+
+#---------------------------------------------------------------------------
+# Project related configuration options
+#---------------------------------------------------------------------------
+OUTPUT_DIRECTORY = .
+CREATE_SUBDIRS = NO
+OUTPUT_LANGUAGE = English
+BRIEF_MEMBER_DESC = YES
+REPEAT_BRIEF = YES
+ABBREVIATE_BRIEF = "The $name class" \
+ "The $name widget" \
+ "The $name file" \
+ is \
+ provides \
+ specifies \
+ contains \
+ represents \
+ a \
+ an \
+ the
+ALWAYS_DETAILED_SEC = YES
+INLINE_INHERITED_MEMB = NO
+FULL_PATH_NAMES = NO
+STRIP_FROM_PATH =
+STRIP_FROM_INC_PATH =
+SHORT_NAMES = NO
+JAVADOC_AUTOBRIEF = YES
+MULTILINE_CPP_IS_BRIEF = NO
+DETAILS_AT_TOP = YES
+INHERIT_DOCS = YES
+DISTRIBUTE_GROUP_DOC = NO
+SEPARATE_MEMBER_PAGES = NO
+TAB_SIZE = 2
+OPTIMIZE_OUTPUT_FOR_C = NO
+OPTIMIZE_OUTPUT_JAVA = NO
+SUBGROUPING = YES
+
+#---------------------------------------------------------------------------
+# Build related configuration options
+#---------------------------------------------------------------------------
+EXTRACT_LOCAL_CLASSES = YES
+EXTRACT_LOCAL_METHODS = NO
+CASE_SENSE_NAMES = NO
+HIDE_SCOPE_NAMES = NO
+SHOW_INCLUDE_FILES = NO
+INLINE_INFO = YES
+SORT_MEMBER_DOCS = YES
+SORT_BRIEF_DOCS = NO
+SORT_BY_SCOPE_NAME = NO
+MAX_INITIALIZER_LINES = 30
+FILE_VERSION_FILTER =
+#---------------------------------------------------------------------------
+# configuration options related to warning and progress messages
+#---------------------------------------------------------------------------
+QUIET = NO
+WARNINGS = YES
+WARN_IF_UNDOCUMENTED = YES
+WARN_IF_DOC_ERROR = YES
+WARN_NO_PARAMDOC = NO
+WARN_FORMAT = "$file:$line: $text"
+WARN_LOGFILE =
+#---------------------------------------------------------------------------
+# configuration options related to the input files
+#---------------------------------------------------------------------------
+RECURSIVE = YES
+EXCLUDE =
+EXCLUDE_SYMLINKS = NO
+EXAMPLE_PATH =
+EXAMPLE_PATTERNS = *
+EXAMPLE_RECURSIVE = NO
+INPUT_FILTER =
+FILTER_PATTERNS =
+FILTER_SOURCE_FILES = NO
+#---------------------------------------------------------------------------
+# configuration options related to source browsing
+#---------------------------------------------------------------------------
+#---------------------------------------------------------------------------
+# configuration options related to the alphabetical class index
+#---------------------------------------------------------------------------
+ALPHABETICAL_INDEX = NO
+COLS_IN_ALPHA_INDEX = 5
+IGNORE_PREFIX =
+#---------------------------------------------------------------------------
+# configuration options related to the HTML output
+#---------------------------------------------------------------------------
+GENERATE_HTML = YES
+HTML_FILE_EXTENSION = .html
+HTML_ALIGN_MEMBERS = YES
+GENERATE_HTMLHELP = NO
+CHM_FILE =
+HHC_LOCATION =
+GENERATE_CHI = NO
+BINARY_TOC = NO
+TOC_EXPAND = YES
+DISABLE_INDEX = NO
+ENUM_VALUES_PER_LINE = 1
+GENERATE_TREEVIEW = YES
+TREEVIEW_WIDTH = 250
+#---------------------------------------------------------------------------
+# configuration options related to the LaTeX output
+#---------------------------------------------------------------------------
+LATEX_OUTPUT = latex
+LATEX_CMD_NAME = latex
+MAKEINDEX_CMD_NAME = makeindex
+COMPACT_LATEX = NO
+PAPER_TYPE = a4wide
+EXTRA_PACKAGES =
+LATEX_HEADER =
+PDF_HYPERLINKS = NO
+USE_PDFLATEX = NO
+LATEX_BATCHMODE = NO
+LATEX_HIDE_INDICES = NO
+#---------------------------------------------------------------------------
+# configuration options related to the RTF output
+#---------------------------------------------------------------------------
+RTF_OUTPUT = rtf
+COMPACT_RTF = NO
+RTF_HYPERLINKS = NO
+RTF_STYLESHEET_FILE =
+RTF_EXTENSIONS_FILE =
+#---------------------------------------------------------------------------
+# configuration options related to the man page output
+#---------------------------------------------------------------------------
+MAN_OUTPUT = man
+MAN_EXTENSION = .3
+MAN_LINKS = NO
+#---------------------------------------------------------------------------
+# configuration options related to the XML output
+#---------------------------------------------------------------------------
+XML_OUTPUT = xml
+XML_SCHEMA =
+XML_DTD =
+XML_PROGRAMLISTING = YES
+#---------------------------------------------------------------------------
+# configuration options for the AutoGen Definitions output
+#---------------------------------------------------------------------------
+GENERATE_AUTOGEN_DEF = NO
+#---------------------------------------------------------------------------
+# configuration options related to the Perl module output
+#---------------------------------------------------------------------------
+PERLMOD_LATEX = NO
+PERLMOD_PRETTY = YES
+PERLMOD_MAKEVAR_PREFIX =
+#---------------------------------------------------------------------------
+# Configuration options related to the preprocessor
+#---------------------------------------------------------------------------
+ENABLE_PREPROCESSING = YES
+MACRO_EXPANSION = YES
+EXPAND_ONLY_PREDEF = YES
+SEARCH_INCLUDES = YES
+INCLUDE_PATH =
+INCLUDE_FILE_PATTERNS =
+EXPAND_AS_DEFINED =
+SKIP_FUNCTION_MACROS = YES
+#---------------------------------------------------------------------------
+# Configuration::additions related to external references
+#---------------------------------------------------------------------------
+TAGFILES = \
+ ${DYNAMICSJRLJAPAN_DOCDIR}/html/dynamicsJRLJapan.doxytag=${DYNAMICSJRLJAPAN_DOCDIR}/html \
+ ${HRP2DYNAMICS_DOCDIR}/html/hrp2Dynamics.doxytag=${HRP2DYNAMICS_DOCDIR}/html \
+ ${DYNAMIC_GRAPH_DOCDIR}/html/dynamic-graph.doxytag=${DYNAMIC_GRAPH_DOCDIR}/html \
+ ${SOT_CORE_DOCDIR}/html/sot-core.doxytag=${SOT_CORE_DOCDIR}/html \
+ ${MATRIXABSTRACTLAYER_DOCDIR}/html/MatrixAbstractLayer.doxytag=${MATRIXABSTRACTLAYER_DOCDIR}/html
+GENERATE_TAGFILE = sot-dynamic.doxytag
+ALLEXTERNALS = NO
+EXTERNAL_GROUPS = YES
+PERL_PATH = /usr/bin/perl
+#---------------------------------------------------------------------------
+# Configuration options related to the dot tool
+#---------------------------------------------------------------------------
+#---------------------------------------------------------------------------
+# Configuration::additions related to the search engine
+#---------------------------------------------------------------------------
+SEARCHENGINE = NO
+
+
+
+
+#---------------------------------------------------------------------------
+# Project related configuration options
+#---------------------------------------------------------------------------
+PROJECT_NAME = "${PROJECT_NAME} library documentation"
+PROJECT_NUMBER = ${PROJECT_VERSION}
+
+#---------------------------------------------------------------------------
+# Build related configuration options
+#---------------------------------------------------------------------------
+EXTRACT_ALL = YES
+EXTRACT_PRIVATE = YES
+EXTRACT_STATIC = NO
+HIDE_UNDOC_MEMBERS = YES
+HIDE_UNDOC_CLASSES = YES
+HIDE_FRIEND_COMPOUNDS = YES
+HIDE_IN_BODY_DOCS = NO
+INTERNAL_DOCS = NO
+SHOW_INCLUDE_FILES = NO
+GENERATE_TODOLIST = YES
+GENERATE_TESTLIST = NO
+GENERATE_BUGLIST = NO
+GENERATE_DEPRECATEDLIST= YES
+ENABLED_SECTIONS =
+SHOW_USED_FILES = NO
+SHOW_DIRECTORIES = NO
+#---------------------------------------------------------------------------
+# configuration options related to warning and progress messages
+#---------------------------------------------------------------------------
+#---------------------------------------------------------------------------
+# configuration options related to the input files
+#---------------------------------------------------------------------------
+FILE_PATTERNS = *.hh *.idl
+EXCLUDE_PATTERNS =
+INPUT = ${${PROJECT_NAME}_SOURCE_DIR}/include \
+ ${CMAKE_CURRENT_SOURCE_DIR}/additionalDoc
+
+#---------------------------------------------------------------------------
+# configuration options related to inserting images
+#---------------------------------------------------------------------------
+IMAGE_PATH = ${${PROJECT_NAME}_SOURCE_DIR}/doc/figures
+#---------------------------------------------------------------------------
+# configuration options related to source browsing
+#---------------------------------------------------------------------------
+SOURCE_BROWSER = NO
+INLINE_SOURCES = NO
+STRIP_CODE_COMMENTS = YES
+REFERENCED_BY_RELATION = NO
+REFERENCES_RELATION = NO
+VERBATIM_HEADERS = NO
+#---------------------------------------------------------------------------
+# configuration options related to the alphabetical class index
+#---------------------------------------------------------------------------
+#---------------------------------------------------------------------------
+# configuration options related to the HTML output
+#---------------------------------------------------------------------------
+HTML_OUTPUT = html
+HTML_STYLESHEET = ${CMAKE_CURRENT_SOURCE_DIR}/package.css
+HTML_HEADER = ${CMAKE_CURRENT_SOURCE_DIR}/header.html
+HTML_FOOTER = ${CMAKE_CURRENT_SOURCE_DIR}/footer.html
+
+#---------------------------------------------------------------------------
+# configuration options related to the LaTeX output
+#---------------------------------------------------------------------------
+GENERATE_LATEX = NO
+USE_PDFLATEX = YES
+#---------------------------------------------------------------------------
+# configuration options related to the RTF output
+#---------------------------------------------------------------------------
+GENERATE_RTF = NO
+#---------------------------------------------------------------------------
+# configuration options related to the man page output
+#---------------------------------------------------------------------------
+GENERATE_MAN = NO
+#---------------------------------------------------------------------------
+# configuration options related to the XML output
+#---------------------------------------------------------------------------
+GENERATE_XML = NO
+#---------------------------------------------------------------------------
+# configuration options for the AutoGen Definitions output
+#---------------------------------------------------------------------------
+GENERATE_AUTOGEN_DEF = NO
+#---------------------------------------------------------------------------
+# configuration options related to the Perl module output
+#---------------------------------------------------------------------------
+GENERATE_PERLMOD = NO
+#---------------------------------------------------------------------------
+# Configuration options related to the preprocessor
+#---------------------------------------------------------------------------
+#---------------------------------------------------------------------------
+# Configuration::additions related to external references
+#---------------------------------------------------------------------------
+#---------------------------------------------------------------------------
+# Configuration options related to the dot tool
+#---------------------------------------------------------------------------
+CLASS_DIAGRAMS = YES
+HIDE_UNDOC_RELATIONS = YES
+HAVE_DOT = YES
+CLASS_GRAPH = NO
+COLLABORATION_GRAPH = YES
+GROUP_GRAPHS = YES
+UML_LOOK = NO
+TEMPLATE_RELATIONS = NO
+INCLUDE_GRAPH = YES
+INCLUDED_BY_GRAPH = YES
+CALL_GRAPH = NO
+GRAPHICAL_HIERARCHY = YES
+DIRECTORY_GRAPH = YES
+DOT_IMAGE_FORMAT = png
+DOT_PATH =
+DOTFILE_DIRS =
+MAX_DOT_GRAPH_DEPTH = 1000
+DOT_TRANSPARENT = NO
+DOT_MULTI_TARGETS = YES
+GENERATE_LEGEND = YES
+DOT_CLEANUP = YES
diff --git a/doc/pictures/footer.txt b/doc/pictures/footer.txt
new file mode 100644
index 0000000..0a7cd95
--- /dev/null
+++ b/doc/pictures/footer.txt
@@ -0,0 +1,2 @@
+Copy in this directory the image you wish to use in the footer of the documentation or edit file sot-dynamic/doc/footer.html and
+remove this file.
diff --git a/include/sot-dynamic/angle-estimator.h b/include/sot-dynamic/angle-estimator.h
new file mode 100644
index 0000000..f8e1f39
--- /dev/null
+++ b/include/sot-dynamic/angle-estimator.h
@@ -0,0 +1,122 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: AngleEstimator.h
+ * Project: SOT
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+
+
+#ifndef __SOT_ANGLE_ESTIMATOR_H__
+#define __SOT_ANGLE_ESTIMATOR_H__
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+# if defined (angle_estimator_EXPORTS)
+# define SOTANGLEESTIMATOR_EXPORT __declspec(dllexport)
+# else
+# define SOTANGLEESTIMATOR_EXPORT __declspec(dllimport)
+# endif
+#else
+# define SOTANGLEESTIMATOR_EXPORT
+#endif
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/* Matrix */
+#include <MatrixAbstractLayer/boost.h>
+namespace ml = maal::boost;
+
+/* SOT */
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/signal-ptr.h>
+#include <dynamic-graph/signal-time-dependent.h>
+#include <sot-core/matrix-homogeneous.h>
+#include <sot-core/vector-roll-pitch-yaw.h>
+#include <sot-core/matrix-rotation.h>
+
+/* STD */
+#include <string>
+
+
+namespace sot {
+namespace dg = dynamicgraph;
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+class SOTANGLEESTIMATOR_EXPORT AngleEstimator
+:public dg::Entity
+{
+ public:
+ static const std::string CLASS_NAME;
+
+ public: /* --- CONSTRUCTION --- */
+
+ AngleEstimator( const std::string& name );
+ virtual ~AngleEstimator( void );
+
+ public: /* --- SIGNAL --- */
+
+ dg::SignalPtr<MatrixRotation,int> sensorWorldRotationSIN; // estimate(worldRc)
+ dg::SignalPtr<MatrixHomogeneous,int> sensorEmbeddedPositionSIN; // waistRchest
+ dg::SignalPtr<MatrixHomogeneous,int> contactWorldPositionSIN; // estimate(worldRf)
+ dg::SignalPtr<MatrixHomogeneous,int> contactEmbeddedPositionSIN; // waistRleg
+ dg::SignalTimeDependent<ml::Vector,int> anglesSOUT; // [ flex1 flex2 yaw_drift ]
+ dg::SignalTimeDependent<MatrixRotation,int> flexibilitySOUT; // footRleg
+ dg::SignalTimeDependent<MatrixRotation,int> driftSOUT; // Ryaw = worldRc est(wRc)^-1
+ dg::SignalTimeDependent<MatrixRotation,int> sensorWorldRotationSOUT; // worldRc
+ dg::SignalTimeDependent<MatrixRotation,int> waistWorldRotationSOUT; // worldRwaist
+ dg::SignalTimeDependent<MatrixHomogeneous,int> waistWorldPositionSOUT; // worldMwaist
+ dg::SignalTimeDependent<ml::Vector,int> waistWorldPoseRPYSOUT; // worldMwaist
+
+ public: /* --- FUNCTIONS --- */
+ ml::Vector& computeAngles( ml::Vector& res,
+ const int& time );
+ MatrixRotation& computeFlexibilityFromAngles( MatrixRotation& res,
+ const int& time );
+ MatrixRotation& computeDriftFromAngles( MatrixRotation& res,
+ const int& time );
+ MatrixRotation& computeSensorWorldRotation( MatrixRotation& res,
+ const int& time );
+ MatrixRotation& computeWaistWorldRotation( MatrixRotation& res,
+ const int& time );
+ MatrixHomogeneous& computeWaistWorldPosition( MatrixHomogeneous& res,
+ const int& time );
+ ml::Vector& computeWaistWorldPoseRPY( ml::Vector& res,
+ const int& time );
+
+ public: /* --- PARAMS --- */
+ bool fromSensor;
+
+ virtual void commandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os );
+
+
+};
+
+
+} // namespace sot
+
+
+
+#endif // #ifndef __SOT_ANGLE_ESTIMATOR_H__
+
diff --git a/include/sot-dynamic/dynamic-hrp2.h b/include/sot-dynamic/dynamic-hrp2.h
new file mode 100644
index 0000000..51b1e93
--- /dev/null
+++ b/include/sot-dynamic/dynamic-hrp2.h
@@ -0,0 +1,89 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: Dynamic.h
+ * Project: SOT
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+
+
+#ifndef __SOT_DYNAMIC_HRP2_H__
+#define __SOT_DYNAMIC_HRP2_H__
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+
+/* SOT */
+#include <sot-dynamic/dynamic.h>
+
+/* JRL dynamic */
+#include <hrp2Dynamics/hrp2OptHumanoidDynamicRobot.h>
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+# if defined (dynamic-hrp2_EXPORTS)
+# define SOTDYNAMICHRP2_EXPORT __declspec(dllexport)
+# else
+# define SOTDYNAMICHRP2_EXPORT __declspec(dllimport)
+# endif
+#else
+# define SOTDYNAMICHRP2_EXPORT
+#endif
+
+
+
+namespace sot {
+namespace dg = dynamicgraph;
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/*! @ingroup signals
+ \brief Modification of the classic dynamic class to get the
+ optimized version of the kinematics computations.
+*/
+
+class SOTDYNAMICHRP2_EXPORT DynamicHrp2
+:public Dynamic
+{
+
+ public:
+ static const std::string CLASS_NAME;
+
+ public: /* --- CONSTRUCTION --- */
+
+ DynamicHrp2( const std::string& name );
+ virtual ~DynamicHrp2( void );
+
+ public: /* --- MODEL CREATION --- */
+
+ void buildModelHrp2( void );
+
+
+};
+
+
+} // namespace sot
+
+
+
+#endif // #ifndef __SOT_DYNAMIC_HRP2_H__
+
diff --git a/include/sot-dynamic/dynamic.h b/include/sot-dynamic/dynamic.h
new file mode 100644
index 0000000..d4ef7c9
--- /dev/null
+++ b/include/sot-dynamic/dynamic.h
@@ -0,0 +1,233 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: Dynamic.h
+ * Project: SOT
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+
+
+#ifndef __SOT_DYNAMIC_H__
+#define __SOT_DYNAMIC_H__
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#include <MatrixAbstractLayer/MatrixAbstractLayer.h>
+/* Matrix */
+#include <MatrixAbstractLayer/boost.h>
+namespace ml = maal::boost;
+
+/* JRL dynamic */
+#include <robotDynamics/jrlHumanoidDynamicRobot.h>
+#include <dynamicsJRLJapan/dynamicsJRLJapanFactory.h>
+namespace djj = dynamicsJRLJapan;
+
+/* SOT */
+#include <sot-core/flags.h>
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/pool.h>
+#include <dynamic-graph/signal-ptr.h>
+#include <dynamic-graph/signal-time-dependent.h>
+#include <sot-core/exception-dynamic.h>
+#include <sot-core/matrix-homogeneous.h>
+
+/* STD */
+#include <string>
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+# if defined (dynamic_EXPORTS)
+# define SOTDYNAMIC_EXPORT __declspec(dllexport)
+# else
+# define SOTDYNAMIC_EXPORT __declspec(dllimport)
+# endif
+#else
+# define SOTDYNAMIC_EXPORT
+#endif
+
+
+namespace sot {
+namespace dg = dynamicgraph;
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/*! @ingroup signals
+ \brief This class provides an inverse dynamic model of the robot.
+ More precisely it wraps the newton euler algorithm implemented
+ by the dynamicsJRLJapan library to make it accessible in the stack of tasks.
+ The robot is described by a VRML file.
+*/
+
+class SOTDYNAMIC_EXPORT Dynamic
+:public dg::Entity
+{
+ public:
+ static const std::string CLASS_NAME;
+
+ protected:
+ public:
+
+ /*! \name Fields to access dynamicsJRLJapan Library
+ @{
+ */
+
+ /*! \brief Abstract pointer on the structure.
+ Ultimately we should be able to use only the abstract
+ interface and do not care about the implementation.
+ */
+ CjrlHumanoidDynamicRobot* m_HDR;
+
+
+ int debugInertia;
+
+ /*! \brief Fields to access the humanoid model
+ @{ */
+
+ /*! \brief Directory where the VRML humanoid model is stored */
+ std::string vrmlDirectory;
+ /*! \brief Name of the root's robot model file */
+ std::string vrmlMainFile;
+ /*! \brief Name of the name specifying which end-effector is
+ the head, the feet and so on... */
+ std::string xmlSpecificityFile;
+ /*! \brief Name of the name specifying which end-effector is
+ the head, the feet and so on... */
+ std::string xmlRankFile;
+ /*! @} */
+
+ /*! @} */
+ bool init;
+ std::list< dg::SignalBase<int>* > genericSignalRefs;
+
+ public: /* --- CONSTRUCTION --- */
+
+ Dynamic( const std::string& name, bool build=true );
+ virtual ~Dynamic( void );
+
+ public: /* --- MODEL CREATION --- */
+
+ virtual void buildModel( void );
+
+ void setVrmlDirectory( const std::string& filename );
+ void setVrmlMainFile( const std::string& filename );
+ void setXmlSpecificityFile( const std::string& filename );
+ void setXmlRankFile( const std::string& filename );
+ void parseConfigFiles( void );
+
+ public: /* --- SIGNAL ACTIVATION --- */
+ dg::SignalTimeDependent< ml::Matrix,int > &
+ createEndeffJacobianSignal( const std::string& signame,
+ const unsigned int & bodyRank );
+ dg::SignalTimeDependent< ml::Matrix,int > &
+ createJacobianSignal( const std::string& signame,
+ const unsigned int & bodyRank );
+ void destroyJacobianSignal( const std::string& signame );
+ dg::SignalTimeDependent< MatrixHomogeneous,int >&
+ createPositionSignal( const std::string& signame,
+ const unsigned int & bodyRank );
+ void destroyPositionSignal( const std::string& signame );
+ dg::SignalTimeDependent< ml::Vector,int >&
+ createAccelerationSignal( const std::string& signame,
+ const unsigned int & bodyRank );
+ void destroyAccelerationSignal( const std::string& signame );
+
+ bool zmpActivation( void ) { std::string Property("ComputeZMP");
+ std::string Value; m_HDR->getProperty(Property,Value); return (Value=="true");}
+ void zmpActivation( const bool& b ) { std::string Property("ComputeZMP");
+ std::string Value; if (b) Value="true"; else Value="false"; m_HDR->setProperty(Property,Value); }
+ bool comActivation( void ) { std::string Property("ComputeCoM");
+ std::string Value; m_HDR->getProperty(Property,Value); return (Value=="true"); }
+ void comActivation( const bool& b ) { std::string Property("ComputeCoM");
+ std::string Value; if (b) Value="true"; else Value="false"; m_HDR->setProperty(Property,Value); }
+
+ public: /* --- SIGNAL --- */
+
+ dg::SignalPtr<ml::Vector,int> jointPositionSIN;
+ dg::SignalPtr<ml::Vector,int> freeFlyerPositionSIN;
+ dg::SignalPtr<ml::Vector,int> jointVelocitySIN;
+ dg::SignalPtr<ml::Vector,int> freeFlyerVelocitySIN;
+ dg::SignalPtr<ml::Vector,int> jointAccelerationSIN;
+ dg::SignalPtr<ml::Vector,int> freeFlyerAccelerationSIN;
+
+ // protected:
+ public:
+ typedef int Dummy;
+ dg::SignalTimeDependent<Dummy,int> firstSINTERN;
+ dg::SignalTimeDependent<Dummy,int> newtonEulerSINTERN;
+
+ int& computeNewtonEuler( int& dummy,int time );
+ int& initNewtonEuler( int& dummy,int time );
+
+ public:
+ dg::SignalTimeDependent<ml::Vector,int> zmpSOUT;
+ dg::SignalTimeDependent<ml::Matrix,int> JcomSOUT;
+ dg::SignalTimeDependent<ml::Vector,int> comSOUT;
+ dg::SignalTimeDependent<ml::Matrix,int> inertiaSOUT;
+
+ dg::SignalTimeDependent<ml::Matrix,int>& jacobiansSOUT( const std::string& name );
+ dg::SignalTimeDependent<MatrixHomogeneous,int>& positionsSOUT( const std::string& name );
+ dg::SignalTimeDependent<ml::Vector,int>& accelerationsSOUT( const std::string& name );
+
+ dg::SignalTimeDependent<double,int> footHeightSOUT;
+ dg::SignalTimeDependent<ml::Vector,int> upperJlSOUT;
+ dg::SignalTimeDependent<ml::Vector,int> lowerJlSOUT;
+
+ dg::Signal<ml::Vector,int> inertiaRotorSOUT;
+ dg::Signal<ml::Vector,int> gearRatioSOUT;
+ dg::SignalTimeDependent<ml::Matrix,int> inertiaRealSOUT;
+ dg::SignalTimeDependent<ml::Vector,int> MomentaSOUT;
+ dg::SignalTimeDependent<ml::Vector,int> AngularMomentumSOUT;
+
+ protected:
+ ml::Vector& computeZmp( ml::Vector& res,int time );
+ ml::Vector& computeMomenta( ml::Vector &res, int time);
+ ml::Vector& computeAngularMomentum( ml::Vector &res, int time);
+ ml::Matrix& computeJcom( ml::Matrix& res,int time );
+ ml::Vector& computeCom( ml::Vector& res,int time );
+ ml::Matrix& computeInertia( ml::Matrix& res,int time );
+ ml::Matrix& computeInertiaReal( ml::Matrix& res,int time );
+ double& computeFootHeight( double& res,int time );
+
+ ml::Matrix& computeGenericJacobian( CjrlJoint* j,ml::Matrix& res,int time );
+ ml::Matrix& computeGenericEndeffJacobian( CjrlJoint* j,ml::Matrix& res,int time );
+ MatrixHomogeneous& computeGenericPosition( CjrlJoint* j,MatrixHomogeneous& res,int time );
+ ml::Vector& computeGenericAcceleration( CjrlJoint* j,ml::Vector& res,int time );
+
+ ml::Vector& getUpperJointLimits( ml::Vector& res,const int& time );
+ ml::Vector& getLowerJointLimits( ml::Vector& res,const int& time );
+
+
+ public: /* --- PARAMS --- */
+ virtual void commandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os );
+
+
+};
+
+
+} // namespace sot
+
+
+
+#endif // #ifndef __SOT_DYNAMIC_H__
+
diff --git a/include/sot-dynamic/force-compensation.h b/include/sot-dynamic/force-compensation.h
new file mode 100644
index 0000000..d16390f
--- /dev/null
+++ b/include/sot-dynamic/force-compensation.h
@@ -0,0 +1,203 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: ForceCompensation.h
+ * Project: SOT
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+
+
+#ifndef __SOT_SOTFORCECOMPENSATION_H__
+#define __SOT_SOTFORCECOMPENSATION_H__
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/* Matrix */
+#include <MatrixAbstractLayer/boost.h>
+namespace ml = maal::boost;
+
+/* SOT */
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/signal-ptr.h>
+#include <dynamic-graph/signal-time-dependent.h>
+#include <sot-core/matrix-rotation.h>
+#include <sot-core/matrix-force.h>
+#include <sot-core/matrix-homogeneous.h>
+
+/* STD */
+#include <string>
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+# if defined (force_compensation_EXPORTS)
+# define SOTFORCECOMPENSATION_EXPORT __declspec(dllexport)
+# else
+# define SOTFORCECOMPENSATION_EXPORT __declspec(dllimport)
+# endif
+#else
+# define SOTFORCECOMPENSATION_EXPORT
+#endif
+
+
+namespace sot {
+namespace dg = dynamicgraph;
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+class SOTFORCECOMPENSATION_EXPORT ForceCompensation
+{
+ private:
+ static MatrixRotation I3;
+ protected:
+ bool usingPrecompensation;
+
+ public:
+ ForceCompensation( void );
+ static MatrixForce& computeHandXworld(
+ const MatrixRotation & worldRhand,
+ const ml::Vector & transSensorCom,
+ MatrixForce& res );
+
+
+ static MatrixForce& computeHandVsensor( const MatrixRotation & sensorRhand,
+ MatrixForce& res );
+ static MatrixForce& computeSensorXhand( const MatrixRotation & sensorRhand,
+ const ml::Vector & transSensorCom,
+ MatrixForce& res );
+/* static ml::Matrix& computeInertiaSensor( const ml::Matrix& inertiaJoint, */
+/* const MatrixForce& sensorXhand, */
+/* ml::Matrix& res ); */
+
+ static ml::Vector& computeTorsorCompensated( const ml::Vector& torqueInput,
+ const ml::Vector& torquePrecompensation,
+ const ml::Vector& gravity,
+ const MatrixForce& handXworld,
+ const MatrixForce& handVsensor,
+ const ml::Matrix& gainSensor,
+ const ml::Vector& momentum,
+ ml::Vector& res );
+
+ static ml::Vector& crossProduct_V_F( const ml::Vector& velocity,
+ const ml::Vector& force,
+ ml::Vector& res );
+ static ml::Vector& computeMomentum( const ml::Vector& velocity,
+ const ml::Vector& acceleration,
+ const MatrixForce& sensorXhand,
+ const ml::Matrix& inertiaJoint,
+ ml::Vector& res );
+
+ static ml::Vector& computeDeadZone( const ml::Vector& torqueInput,
+ const ml::Vector& deadZoneLimit,
+ ml::Vector& res );
+
+ public: // CALIBRATION
+
+ std::list<ml::Vector> torsorList;
+ std::list<MatrixRotation> rotationList;
+
+ void clearCalibration( void );
+ void addCalibrationValue( const ml::Vector& torsor,
+ const MatrixRotation & worldRhand );
+
+ ml::Vector calibrateTransSensorCom( const ml::Vector& gravity,
+ const MatrixRotation& handRsensor );
+ ml::Vector calibrateGravity( const MatrixRotation& handRsensor,
+ bool precompensationCalibration = false,
+ const MatrixRotation& hand0Rsensor = I3 );
+
+
+
+};
+
+/* --------------------------------------------------------------------- */
+/* --- PLUGIN ---------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+class SOTFORCECOMPENSATION_EXPORT ForceCompensationPlugin
+:public dg::Entity, public ForceCompensation
+{
+ public:
+ static const std::string CLASS_NAME;
+ bool calibrationStarted;
+
+
+ public: /* --- CONSTRUCTION --- */
+
+ ForceCompensationPlugin( const std::string& name );
+ virtual ~ForceCompensationPlugin( void );
+
+ public: /* --- SIGNAL --- */
+
+ /* --- INPUTS --- */
+ dg::SignalPtr<ml::Vector,int> torsorSIN;
+ dg::SignalPtr<MatrixRotation,int> worldRhandSIN;
+
+ /* --- CONSTANTS --- */
+ dg::SignalPtr<MatrixRotation,int> handRsensorSIN;
+ dg::SignalPtr<ml::Vector,int> translationSensorComSIN;
+ dg::SignalPtr<ml::Vector,int> gravitySIN;
+ dg::SignalPtr<ml::Vector,int> precompensationSIN;
+ dg::SignalPtr<ml::Matrix,int> gainSensorSIN;
+ dg::SignalPtr<ml::Vector,int> deadZoneLimitSIN;
+ dg::SignalPtr<ml::Vector,int> transSensorJointSIN;
+ dg::SignalPtr<ml::Matrix,int> inertiaJointSIN;
+
+ dg::SignalPtr<ml::Vector,int> velocitySIN;
+ dg::SignalPtr<ml::Vector,int> accelerationSIN;
+
+ /* --- INTERMEDIATE OUTPUTS --- */
+ dg::SignalTimeDependent<MatrixForce,int> handXworldSOUT;
+ dg::SignalTimeDependent<MatrixForce,int> handVsensorSOUT;
+ dg::SignalPtr<ml::Vector,int> torsorDeadZoneSIN;
+
+ dg::SignalTimeDependent<MatrixForce,int> sensorXhandSOUT;
+ //dg::SignalTimeDependent<ml::Matrix,int> inertiaSensorSOUT;
+ dg::SignalTimeDependent<ml::Vector,int> momentumSOUT;
+ dg::SignalPtr<ml::Vector,int> momentumSIN;
+
+ /* --- OUTPUTS --- */
+ dg::SignalTimeDependent<ml::Vector,int> torsorCompensatedSOUT;
+ dg::SignalTimeDependent<ml::Vector,int> torsorDeadZoneSOUT;
+
+ typedef int sotDummyType;
+ dg::SignalTimeDependent<sotDummyType,int> calibrationTrigerSOUT;
+
+ public: /* --- COMMANDLINE --- */
+
+ sotDummyType& calibrationTriger( sotDummyType& dummy,int time );
+
+
+ virtual void commandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os );
+
+
+
+};
+
+
+} // namaspace sot
+
+
+
+#endif // #ifndef __SOT_SOTFORCECOMPENSATION_H__
+
diff --git a/include/sot-dynamic/integrator-force-exact.h b/include/sot-dynamic/integrator-force-exact.h
new file mode 100644
index 0000000..37e28a4
--- /dev/null
+++ b/include/sot-dynamic/integrator-force-exact.h
@@ -0,0 +1,101 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: IntegratorForceExact.h
+ * Project: SOT
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+
+
+#ifndef __SOT_SOTINTEGRATORFORCEEXACT_H__
+#define __SOT_SOTINTEGRATORFORCEEXACT_H__
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/* Matrix */
+#include <MatrixAbstractLayer/boost.h>
+namespace ml = maal::boost;
+
+/* SOT */
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/signal-ptr.h>
+#include <dynamic-graph/signal-time-dependent.h>
+#include <sot-core/matrix-homogeneous.h>
+#include <sot-core/vector-roll-pitch-yaw.h>
+#include <sot-core/matrix-rotation.h>
+#include <sot-dynamic/integrator-force.h>
+
+/* STD */
+#include <string>
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+# if defined (integrator_force_exact_EXPORTS)
+# define SOTINTEGRATORFORCEEXACT_EXPORT __declspec(dllexport)
+# else
+# define SOTINTEGRATORFORCEEXACT_EXPORT __declspec(dllimport)
+# endif
+#else
+# define SOTINTEGRATORFORCEEXACT_EXPORT
+#endif
+
+
+namespace sot {
+namespace dg = dynamicgraph;
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+class SOTINTEGRATORFORCEEXACT_EXPORT IntegratorForceExact
+:public IntegratorForce
+{
+ public:
+ static const std::string CLASS_NAME;
+
+ protected:
+
+ public: /* --- CONSTRUCTION --- */
+
+ IntegratorForceExact( const std::string& name );
+ virtual ~IntegratorForceExact( void );
+
+ public: /* --- SIGNAL --- */
+
+
+ public: /* --- FUNCTIONS --- */
+ ml::Vector& computeVelocityExact( ml::Vector& res,
+ const int& time );
+
+/* public: /\* --- PARAMS --- *\/ */
+/* virtual void commandLine( const std::string& cmdLine, */
+/* std::istringstream& cmdArgs, */
+/* std::ostream& os ); */
+
+
+};
+
+
+} // namespace sot
+
+
+
+#endif // #ifndef __SOT_SOTINTEGRATORFORCEEXACT_H__
+
diff --git a/include/sot-dynamic/integrator-force-rk4.h b/include/sot-dynamic/integrator-force-rk4.h
new file mode 100644
index 0000000..5cdf365
--- /dev/null
+++ b/include/sot-dynamic/integrator-force-rk4.h
@@ -0,0 +1,102 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: IntegratorForceRK4.h
+ * Project: SOT
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+
+
+#ifndef __SOT_SOTINTEGRATORFORCERK4_H__
+#define __SOT_SOTINTEGRATORFORCERK4_H__
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/* Matrix */
+#include <MatrixAbstractLayer/boost.h>
+namespace ml = maal::boost;
+
+/* SOT */
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/signal-ptr.h>
+#include <dynamic-graph/signal-time-dependent.h>
+#include <sot-core/matrix-homogeneous.h>
+#include <sot-core/vector-roll-pitch-yaw.h>
+#include <sot-core/matrix-rotation.h>
+#include <sot-dynamic/integrator-force.h>
+
+/* STD */
+#include <string>
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+# if defined (integrator_force_rk4_EXPORTS)
+# define SOTINTEGRATORFORCERK4_EXPORT __declspec(dllexport)
+# else
+# define SOTINTEGRATORFORCERK4_EXPORT __declspec(dllimport)
+# endif
+#else
+# define SOTINTEGRATORFORCERK4_EXPORT
+#endif
+
+
+namespace sot {
+namespace dg = dynamicgraph;
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+class SOTINTEGRATORFORCERK4_EXPORT IntegratorForceRK4
+:public IntegratorForce
+{
+ public:
+ static const std::string CLASS_NAME;
+
+ protected:
+
+ public: /* --- CONSTRUCTION --- */
+
+ IntegratorForceRK4( const std::string& name );
+ virtual ~IntegratorForceRK4( void );
+
+ public: /* --- SIGNAL --- */
+
+
+ public: /* --- FUNCTIONS --- */
+ ml::Vector& computeDerivativeRK4( ml::Vector& res,
+ const int& time );
+
+/* public: /\* --- PARAMS --- *\/ */
+/* virtual void commandLine( const std::string& cmdLine, */
+/* std::istringstream& cmdArgs, */
+/* std::ostream& os ); */
+
+
+};
+
+
+} // namespace sot
+
+
+
+
+#endif // #ifndef __SOT_SOTINTEGRATORFORCERK4_H__
+
diff --git a/include/sot-dynamic/integrator-force.h b/include/sot-dynamic/integrator-force.h
new file mode 100644
index 0000000..91d00cb
--- /dev/null
+++ b/include/sot-dynamic/integrator-force.h
@@ -0,0 +1,121 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: IntegratorForce.h
+ * Project: SOT
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+
+
+#ifndef __SOT_SOTINTEGRATORFORCE_H__
+#define __SOT_SOTINTEGRATORFORCE_H__
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/* Matrix */
+#include <MatrixAbstractLayer/boost.h>
+namespace ml = maal::boost;
+
+/* SOT */
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/signal-ptr.h>
+#include <dynamic-graph/signal-time-dependent.h>
+#include <sot-core/matrix-homogeneous.h>
+#include <sot-core/vector-roll-pitch-yaw.h>
+#include <sot-core/matrix-rotation.h>
+
+/* STD */
+#include <string>
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+# if defined (integrator_force_EXPORTS)
+# define SOTINTEGRATORFORCE_EXPORT __declspec(dllexport)
+# else
+# define SOTINTEGRATORFORCE_EXPORT __declspec(dllimport)
+# endif
+#else
+# define SOTINTEGRATORFORCE_EXPORT
+#endif
+
+
+namespace sot {
+namespace dg = dynamicgraph;
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+class SOTINTEGRATORFORCE_EXPORT IntegratorForce
+:public dg::Entity
+{
+ public:
+ static const std::string CLASS_NAME;
+
+ protected:
+ double timeStep;
+ static const double TIME_STEP_DEFAULT ; // = 5e-3
+
+
+ public: /* --- CONSTRUCTION --- */
+
+ IntegratorForce( const std::string& name );
+ virtual ~IntegratorForce( void );
+
+ public: /* --- SIGNAL --- */
+
+ dg::SignalPtr<ml::Vector,int> forceSIN;
+ dg::SignalPtr<ml::Matrix,int> massInverseSIN;
+ dg::SignalPtr<ml::Matrix,int> frictionSIN;
+
+ /* Memory of the previous iteration. The sig is fed by the previous
+ * computations. */
+ dg::SignalPtr<ml::Vector,int> velocityPrecSIN;
+ dg::SignalTimeDependent<ml::Vector,int> velocityDerivativeSOUT;
+ dg::SignalTimeDependent<ml::Vector,int> velocitySOUT;
+
+ dg::SignalPtr<ml::Matrix,int> massSIN;
+ dg::SignalTimeDependent<ml::Matrix,int> massInverseSOUT;
+
+ public: /* --- FUNCTIONS --- */
+ ml::Vector& computeDerivative( ml::Vector& res,
+ const int& time );
+ ml::Vector& computeIntegral( ml::Vector& res,
+ const int& time );
+
+ ml::Matrix& computeMassInverse( ml::Matrix& res,
+ const int& time );
+
+
+ public: /* --- PARAMS --- */
+ virtual void commandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os );
+
+
+};
+
+
+} // namespace sot
+
+
+
+#endif // #ifndef __SOT_WAISTATTITUDEFROMSENSOR_H__
+
diff --git a/include/sot-dynamic/mass-apparent.h b/include/sot-dynamic/mass-apparent.h
new file mode 100644
index 0000000..5b9c4b7
--- /dev/null
+++ b/include/sot-dynamic/mass-apparent.h
@@ -0,0 +1,97 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: MassApparent.h
+ * Project: SOT
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+
+
+#ifndef __SOT_SOTMASSAPPARENT_H__
+#define __SOT_SOTMASSAPPARENT_H__
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/* Matrix */
+#include <MatrixAbstractLayer/boost.h>
+namespace ml = maal::boost;
+
+/* SOT */
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/signal-ptr.h>
+#include <dynamic-graph/signal-time-dependent.h>
+#include <sot-core/matrix-homogeneous.h>
+#include <sot-core/vector-roll-pitch-yaw.h>
+#include <sot-core/matrix-rotation.h>
+
+/* STD */
+#include <string>
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+# if defined (mass_apparent_EXPORTS)
+# define SOTMASSAPPARENT_EXPORT __declspec(dllexport)
+# else
+# define SOTMASSAPPARENT_EXPORT __declspec(dllimport)
+# endif
+#else
+# define SOTMASSAPPARENT_EXPORT
+#endif
+
+
+namespace sot {
+namespace dg = dynamicgraph;
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+class SOTMASSAPPARENT_EXPORT MassApparent
+:public dg::Entity
+{
+ public:
+ static const std::string CLASS_NAME;
+
+ public: /* --- CONSTRUCTION --- */
+
+ MassApparent( const std::string& name );
+ virtual ~MassApparent( void );
+
+ public: /* --- SIGNAL --- */
+
+ dg::SignalPtr<ml::Matrix,int> jacobianSIN;
+ dg::SignalPtr<ml::Matrix,int> inertiaInverseSIN;
+ dg::SignalTimeDependent<ml::Matrix,int> massInverseSOUT;
+ dg::SignalTimeDependent<ml::Matrix,int> massSOUT;
+
+ dg::SignalPtr<ml::Matrix,int> inertiaSIN;
+ dg::SignalTimeDependent<ml::Matrix,int> inertiaInverseSOUT;
+
+ public: /* --- FUNCTIONS --- */
+ ml::Matrix& computeMassInverse( ml::Matrix& res,const int& time );
+ ml::Matrix& computeMass( ml::Matrix& res,const int& time );
+ ml::Matrix& computeInertiaInverse( ml::Matrix& res,const int& time );
+};
+
+
+}
+
+
+#endif // #ifndef __SOT_SOTMASSAPPARENT_H__
+
diff --git a/include/sot-dynamic/matrix-inertia.h b/include/sot-dynamic/matrix-inertia.h
new file mode 100644
index 0000000..2284a57
--- /dev/null
+++ b/include/sot-dynamic/matrix-inertia.h
@@ -0,0 +1,84 @@
+#ifndef __SOT_SOTMATRIXINERTIA_H__
+#define __SOT_SOTMATRIXINERTIA_H__
+
+#include <ostream>
+
+#include "MatrixAbstractLayer/boost.h"
+#include "MatrixAbstractLayer/MatrixAbstractLayer.h"
+
+#include <sot-core/matrix-twist.h>
+#include <sot-core/matrix-force.h>
+#include <sot-core/matrix-rotation.h>
+#include <sot-core/matrix-homogeneous.h>
+
+namespace dynamicsJRLJapan
+{
+ class HumanoidDynamicMultiBody;
+}
+
+class CjrlHumanoidDynamicRobot;
+class CjrlJoint;
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+# if defined (matrix_inertia_EXPORTS)
+# define SOTMATRIXINERTIA_EXPORT __declspec(dllexport)
+# else
+# define SOTMATRIXINERTIA_EXPORT __declspec(dllimport)
+# endif
+#else
+# define SOTMATRIXINERTIA_EXPORT
+#endif
+
+
+namespace sot {
+namespace dg = dynamicgraph;
+
+/* -------------------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+class SOTMATRIXINERTIA_EXPORT MatrixInertia
+{
+public:
+
+ private:
+ MatrixInertia( void ) {}
+
+ void initParents( void );
+ void initDofTable( void );
+
+ public:
+ MatrixInertia( CjrlHumanoidDynamicRobot* aHDR );
+ ~MatrixInertia( void );
+ void init( CjrlHumanoidDynamicRobot* aHDR );
+
+public:
+
+ void update( void );
+ void computeInertiaMatrix();
+ void getInertiaMatrix(double* A);
+ const maal::boost::Matrix& getInertiaMatrix( void );
+ size_t getDoF() { return joints_.size(); }
+
+private:
+
+ CjrlHumanoidDynamicRobot* aHDR_;
+ dynamicsJRLJapan::HumanoidDynamicMultiBody* aHDMB_;
+ std::vector<CjrlJoint*> joints_;
+ std::vector<int> parentIndex_;
+
+ std::vector< ml::Matrix > Ic;
+ std::vector< ml::Vector > phi;
+ std::vector< sotMatrixTwist > iVpi;
+ std::vector< MatrixForce > iVpiT;
+ ml::Matrix inertia_;
+
+
+};
+
+} // namespace sot
+
+#endif // __SOT_SOTMATRIXINERTIA_H__
diff --git a/include/sot-dynamic/waist-attitude-from-sensor.h b/include/sot-dynamic/waist-attitude-from-sensor.h
new file mode 100644
index 0000000..9b9b2dc
--- /dev/null
+++ b/include/sot-dynamic/waist-attitude-from-sensor.h
@@ -0,0 +1,131 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: WaistAttitudeFromSensor.h
+ * Project: SOT
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+
+
+#ifndef __SOT_WAISTATTITUDEFROMSENSOR_H__
+#define __SOT_WAISTATTITUDEFROMSENSOR_H__
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/* Matrix */
+#include <MatrixAbstractLayer/boost.h>
+namespace ml = maal::boost;
+
+/* SOT */
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/signal-ptr.h>
+#include <dynamic-graph/signal-time-dependent.h>
+#include <sot-core/matrix-homogeneous.h>
+#include <sot-core/vector-roll-pitch-yaw.h>
+
+/* STD */
+#include <string>
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+# if defined (waist_attitue_from_sensor_EXPORTS)
+# define SOTWAISTATTITUDEFROMSENSOR_EXPORT __declspec(dllexport)
+# else
+# define SOTWAISTATTITUDEFROMSENSOR_EXPORT __declspec(dllimport)
+# endif
+#else
+# define SOTWAISTATTITUDEFROMSENSOR_EXPORT
+#endif
+
+
+namespace sot {
+namespace dg = dynamicgraph;
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+class SOTWAISTATTITUDEFROMSENSOR_EXPORT WaistAttitudeFromSensor
+:public dg::Entity
+{
+ public:
+ static const std::string CLASS_NAME;
+
+ public: /* --- CONSTRUCTION --- */
+
+ WaistAttitudeFromSensor( const std::string& name );
+ virtual ~WaistAttitudeFromSensor( void );
+
+ public: /* --- SIGNAL --- */
+
+ VectorRollPitchYaw & computeAttitudeWaist( VectorRollPitchYaw & res,
+ const int& time );
+
+ dg::SignalPtr<MatrixRotation,int> attitudeSensorSIN;
+ dg::SignalPtr<MatrixHomogeneous,int> positionSensorSIN;
+ dg::SignalTimeDependent<VectorRollPitchYaw,int> attitudeWaistSOUT;
+
+
+ public: /* --- PARAMS --- */
+ virtual void commandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os );
+
+
+};
+
+
+class SOTWAISTATTITUDEFROMSENSOR_EXPORT WaistPoseFromSensorAndContact
+:public WaistAttitudeFromSensor
+{
+ public:
+ static const std::string CLASS_NAME;
+
+ protected:
+ bool fromSensor;
+
+ public: /* --- CONSTRUCTION --- */
+
+ WaistPoseFromSensorAndContact( const std::string& name );
+ virtual ~WaistPoseFromSensorAndContact( void );
+
+ public: /* --- SIGNAL --- */
+
+ ml::Vector& computePositionWaist( ml::Vector& res,
+ const int& time );
+
+ dg::SignalPtr<MatrixHomogeneous,int> positionContactSIN;
+ dg::SignalTimeDependent<ml::Vector,int> positionWaistSOUT;
+
+
+ public: /* --- PARAMS --- */
+ virtual void commandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os );
+
+
+};
+
+
+} // namespace sot
+
+
+#endif // #ifndef __SOT_WAISTATTITUDEFROMSENSOR_H__
+
diff --git a/include/sot-dynamic/zmpreffromcom.h b/include/sot-dynamic/zmpreffromcom.h
new file mode 100644
index 0000000..575a526
--- /dev/null
+++ b/include/sot-dynamic/zmpreffromcom.h
@@ -0,0 +1,102 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: ZmprefFromCom.h
+ * Project: SOT
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+
+
+#ifndef __SOT_ZMPREFFROMCOM_H__
+#define __SOT_ZMPREFFROMCOM_H__
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/* Matrix */
+#include <MatrixAbstractLayer/boost.h>
+namespace ml = maal::boost;
+
+/* SOT */
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/signal-ptr.h>
+#include <dynamic-graph/signal-time-dependent.h>
+#include <sot-core/matrix-homogeneous.h>
+
+/* STD */
+#include <string>
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+# if defined (zmpreffromcom_EXPORTS)
+# define SOTZMPREFFROMCOM_EXPORT __declspec(dllexport)
+# else
+# define SOTZMPREFFROMCOM_EXPORT __declspec(dllimport)
+# endif
+#else
+# define SOTZMPREFFROMCOM_EXPORT
+#endif
+
+namespace sot {
+namespace dg = dynamicgraph;
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+class SOTZMPREFFROMCOM_EXPORT ZmprefFromCom
+:public dg::Entity
+{
+ public:
+ static const std::string CLASS_NAME;
+ double dt;
+ const static double DT_DEFAULT; // = 5e-3; // 5ms
+ double footHeight;
+ const static double FOOT_HEIGHT_DEFAULT; // = .105;
+
+ public: /* --- CONSTRUCTION --- */
+
+ ZmprefFromCom( const std::string& name );
+ virtual ~ZmprefFromCom( void );
+
+ public: /* --- SIGNAL --- */
+
+ ml::Vector& computeZmpref( ml::Vector& res,
+ const int& time );
+
+ dg::SignalPtr<MatrixHomogeneous,int> waistPositionSIN;
+ dg::SignalPtr<ml::Vector,int> comPositionSIN;
+ dg::SignalPtr<ml::Vector,int> dcomSIN;
+ dg::SignalTimeDependent<ml::Vector,int> zmprefSOUT;
+
+
+ public: /* --- PARAMS --- */
+ virtual void commandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os );
+
+
+};
+
+
+} // namespace sot
+
+
+#endif // #ifndef __SOT_ZMPREFFROMCOM_H__
+
diff --git a/sot-dynamic.pc.cmake b/sot-dynamic.pc.cmake
new file mode 100644
index 0000000..1810450
--- /dev/null
+++ b/sot-dynamic.pc.cmake
@@ -0,0 +1,13 @@
+prefix=${CMAKE_INSTALL_PREFIX}
+exec_prefix=${install_pkg_prefix}
+libdir=${install_pkg_exec_prefix}/lib
+includedir=${install_pkg_prefix}/include
+datarootdir=${install_pkg_prefix}/share
+docdir=${install_pkg_datarootdir}/doc/${PROJECT_NAME}
+
+Name: ${PROJECT_NAME}
+Description:
+Version: ${PROJECT_VERSION}
+Requires: ${PACKAGE_REQUIREMENTS}
+Libs: ${LIBDIR_KW}${install_pkg_libdir} ${${PROJECT_NAME}_LDFLAGS}
+Cflags: -I${install_pkg_include_dir} ${${PROJECT_NAME}_CXXFLAGS}
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
new file mode 100644
index 0000000..081011d
--- /dev/null
+++ b/src/CMakeLists.txt
@@ -0,0 +1,130 @@
+#
+# Copyright
+#
+
+INCLUDE_DIRECTORIES(${CMAKE_SOURCE_DIR}/include)
+
+IF(CMAKE_BUILD_TYPE STREQUAL "DEBUG")
+ ADD_DEFINITIONS(-DDEBUG=2)
+ENDIF(CMAKE_BUILD_TYPE STREQUAL "DEBUG")
+
+# provide path to library libdynamicsJRLJapan.so
+LINK_DIRECTORIES(${DYNAMICSJRLJAPAN_LIBRARY_DIRS})
+
+
+# provide path to library libhrp2Dynamics.so
+LINK_DIRECTORIES(${HRP2DYNAMICS_LIBRARY_DIRS})
+
+
+# provide path to library libdynamic-graph.so
+LINK_DIRECTORIES(${DYNAMIC_GRAPH_LIBRARY_DIRS})
+
+
+# provide path to library libsot-core.so
+LINK_DIRECTORIES(${SOT_CORE_LIBRARY_DIRS})
+
+
+# provide path to library libMatrixAbstractLayer.so
+LINK_DIRECTORIES(${MATRIXABSTRACTLAYER_LIBRARY_DIRS})
+
+# Add dynamicsJRLJapan compilation flags and link to library libdynamicsJRLJapan.so
+ADD_DEFINITIONS(${DYNAMICSJRLJAPAN_CFLAGS})
+
+IF(WIN32)
+foreach(dlink ${DYNAMICSJRLJAPAN_LDFLAGS})
+ SET ( ${PROJECT_NAME}_src_LDFLAGS "${${PROJECT_NAME}_src_LDFLAGS} ${dlink}")
+endforeach(dlink)
+ENDIF(WIN32)
+
+# Add hrp2Dynamics compilation flags and link to library libhrp2Dynamics.so
+ADD_DEFINITIONS(${HRP2DYNAMICS_CFLAGS})
+
+IF(WIN32)
+foreach(dlink ${HRP2DYNAMICS_LDFLAGS})
+ SET ( ${PROJECT_NAME}_src_LDFLAGS "${${PROJECT_NAME}_src_LDFLAGS} ${dlink}")
+endforeach(dlink)
+ENDIF(WIN32)
+
+# Add dynamic-graph compilation flags and link to library libdynamic-graph.so
+ADD_DEFINITIONS(${DYNAMIC_GRAPH_CFLAGS})
+
+IF(WIN32)
+foreach(dlink ${DYNAMIC_GRAPH_LDFLAGS})
+ SET ( ${PROJECT_NAME}_src_LDFLAGS "${${PROJECT_NAME}_src_LDFLAGS} ${dlink}")
+endforeach(dlink)
+ENDIF(WIN32)
+
+# Add sot-core compilation flags and link to library libsot-core.so
+ADD_DEFINITIONS(${SOT_CORE_CFLAGS})
+
+IF(WIN32)
+foreach(dlink ${SOT_CORE_LDFLAGS})
+ SET ( ${PROJECT_NAME}_src_LDFLAGS "${${PROJECT_NAME}_src_LDFLAGS} ${dlink}")
+endforeach(dlink)
+ENDIF(WIN32)
+
+# Add MatrixAbstractLayer compilation flags and link to library libMatrixAbstractLayer.so
+ADD_DEFINITIONS(${MATRIXABSTRACTLAYER_CFLAGS})
+
+IF(WIN32)
+foreach(dlink ${MATRIXABSTRACTLAYER_LDFLAGS})
+ SET ( ${PROJECT_NAME}_src_LDFLAGS "${${PROJECT_NAME}_src_LDFLAGS} ${dlink}")
+endforeach(dlink)
+ENDIF(WIN32)
+
+SET(libs
+ zmpreffromcom
+ force-compensation
+ integrator-force-exact
+ mass-apparent
+ integrator-force-rk4
+ integrator-force
+ angle-estimator
+ waist-attitude-from-sensor
+)
+
+IF(${_dynamicsJRLJapan_FOUND})
+ LIST(APPEND libs dynamic)
+ENDIF(${_dynamicsJRLJapan_FOUND})
+
+IF(${_hrp2Dynamics_FOUND})
+ LIST(APPEND libs dynamic-hrp2)
+ SET (dynamic-hrp2_plugins_dependencies dynamic)
+ENDIF(${_hrp2Dynamics_FOUND})
+
+SET(integrator-force-rk4_plugins_dependencies integrator-force)
+SET(integrator-force-exact_plugins_dependencies integrator-force)
+
+FOREACH(lib ${libs})
+ ADD_LIBRARY(${lib}
+ SHARED
+ ${lib}.cpp)
+
+ SET_TARGET_PROPERTIES(${lib}
+ PROPERTIES
+ SOVERSION ${PROJECT_VERSION}
+ PREFIX ""
+ INSTALL_RPATH ${CMAKE_INSTALL_PREFIX}/lib/plugin)
+
+ IF (UNIX)
+ TARGET_LINK_LIBRARIES(${lib} ${DYNAMIC_GRAPH_LIBRARIES})
+ TARGET_LINK_LIBRARIES(${lib} ${SOT_CORE_LIBRARIES})
+ TARGET_LINK_LIBRARIES(${lib} ${MATRIXABSTRACTLAYER_LIBRARIES})
+ TARGET_LINK_LIBRARIES(${lib} ${HRP2DYNAMICS_LIBRARIES})
+ TARGET_LINK_LIBRARIES(${lib} ${DYNAMICSJRLJAPAN_LIBRARIES})
+ ENDIF(UNIX)
+
+ IF(WIN32)
+ SET_TARGET_PROPERTIES(${lib}
+ PROPERTIES
+ LINK_FLAGS "${${PROJECT_NAME}_src_LDFLAGS}"
+ )
+ ENDIF(WIN32)
+
+ ADD_DEPENDENCIES(${lib} "${${lib}_plugins_dependencies}")
+
+ INSTALL(TARGETS ${lib}
+ DESTINATION ${CMAKE_INSTALL_PREFIX}/lib/plugin)
+ENDFOREACH(lib)
+
+
\ No newline at end of file
diff --git a/src/angle-estimator.cpp b/src/angle-estimator.cpp
new file mode 100644
index 0000000..f4ad420
--- /dev/null
+++ b/src/angle-estimator.cpp
@@ -0,0 +1,325 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: AngleEstimator.h
+ * Project: SOT
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+#include <sot-dynamic/angle-estimator.h>
+#include <sot-core/debug.h>
+#include <dynamic-graph/factory.h>
+
+#include <MatrixAbstractLayer/MatrixAbstractLayer.h>
+
+using namespace sot;
+using namespace dynamicgraph;
+
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AngleEstimator,"AngleEstimator");
+
+AngleEstimator::
+AngleEstimator( const std::string & name )
+ :Entity(name)
+ ,sensorWorldRotationSIN(NULL,"sotAngleEstimator("+name
+ +")::input(MatrixHomo)::sensorWorldRotation")
+ ,sensorEmbeddedPositionSIN(NULL,"sotAngleEstimator("+name
+ +")::input(MatrixHomo)::sensorEmbeddedPosition")
+ ,contactWorldPositionSIN(NULL,"sotAngleEstimator("+name
+ +")::input(MatrixHomo)::contactWorldPosition")
+ ,contactEmbeddedPositionSIN(NULL,"sotAngleEstimator("+name
+ +")::input(MatrixHomo)::contactEmbeddedPosition")
+
+ ,anglesSOUT( boost::bind(&AngleEstimator::computeAngles,this,_1,_2),
+ sensorWorldRotationSIN<<sensorEmbeddedPositionSIN
+ <<contactWorldPositionSIN<<contactEmbeddedPositionSIN,
+ "sotAngleEstimator("+name+")::output(Vector)::angles" )
+ ,flexibilitySOUT( boost::bind(&AngleEstimator::computeFlexibilityFromAngles
+ ,this,_1,_2),
+ anglesSOUT,
+ "sotAngleEstimator("+name+")::output(matrixRotation)::flexibility" )
+ ,driftSOUT( boost::bind(&AngleEstimator::computeDriftFromAngles,this,_1,_2),
+ anglesSOUT,
+ "sotAngleEstimator("+name+")::output(matrixRotation)::drift" )
+ ,sensorWorldRotationSOUT( boost::bind(&AngleEstimator::computeSensorWorldRotation
+ ,this,_1,_2),
+ anglesSOUT<<sensorWorldRotationSIN,
+ "sotAngleEstimator("+name
+ +")::output(matrixRotation)::sensorCorrectedRotation" )
+ ,waistWorldRotationSOUT( boost::bind(&AngleEstimator::computeWaistWorldRotation
+ ,this,_1,_2),
+ sensorWorldRotationSOUT<<sensorEmbeddedPositionSIN,
+ "sotAngleEstimator("+name
+ +")::output(matrixRotation)::waistWorldRotation" )
+ ,waistWorldPositionSOUT( boost::bind(&AngleEstimator::computeWaistWorldPosition
+ ,this,_1,_2),
+ flexibilitySOUT << contactEmbeddedPositionSIN,
+ "sotAngleEstimator("+name
+ +")::output(MatrixHomogeneous)::waistWorldPosition" )
+ ,waistWorldPoseRPYSOUT( boost::bind(&AngleEstimator::computeWaistWorldPoseRPY
+ ,this,_1,_2),
+ waistWorldPositionSOUT,
+ "sotAngleEstimator("+name
+ +")::output(vectorRollPitchYaw)::waistWorldPoseRPY" )
+
+ ,fromSensor(true)
+{
+ sotDEBUGIN(5);
+
+
+ signalRegistration( sensorWorldRotationSIN << sensorEmbeddedPositionSIN
+ << contactWorldPositionSIN << contactEmbeddedPositionSIN
+ << anglesSOUT << flexibilitySOUT
+ << driftSOUT << sensorWorldRotationSOUT
+ << waistWorldRotationSOUT
+ << waistWorldPositionSOUT << waistWorldPoseRPYSOUT );
+
+ sotDEBUGOUT(5);
+}
+
+
+AngleEstimator::
+~AngleEstimator( void )
+{
+ sotDEBUGIN(5);
+
+ sotDEBUGOUT(5);
+ return;
+}
+
+/* --- SIGNALS -------------------------------------------------------------- */
+/* --- SIGNALS -------------------------------------------------------------- */
+/* --- SIGNALS -------------------------------------------------------------- */
+ml::Vector& AngleEstimator::
+computeAngles( ml::Vector& res,
+ const int& time )
+{
+ sotDEBUGIN(15);
+
+ res.resize(3);
+
+ const MatrixRotation &worldestRchest = sensorWorldRotationSIN( time );
+ sotDEBUG(35) << "worldestRchest = " << std::endl << worldestRchest;
+ const MatrixHomogeneous &waistMchest = sensorEmbeddedPositionSIN( time );
+ MatrixRotation waistRchest; waistMchest.extract( waistRchest );
+
+ const MatrixHomogeneous & waistMleg = contactEmbeddedPositionSIN( time );
+ MatrixRotation waistRleg; waistMleg.extract( waistRleg );
+ MatrixRotation chestRleg; waistRchest.transpose().multiply( waistRleg,chestRleg );
+ MatrixRotation worldestRleg; worldestRchest.multiply( chestRleg,worldestRleg );
+
+ sotDEBUG(35) << "worldestRleg = " << std::endl << worldestRleg;
+
+ /* Euler angles with following code: (-z)xy, -z being the yaw drift, x the
+ * first flexibility and y the second flexibility. */
+ const double TOLERANCE_TH = 1e-6;
+
+ const MatrixRotation &R = worldestRleg;
+ if( (fabs(R(0,1))<TOLERANCE_TH)&&(fabs(R(1,1))<TOLERANCE_TH) )
+ {
+ /* This means that cos(X) is very low, ie flex1 is close to 90deg.
+ * I take the case into account, but it is bloody never going
+ * to happens. */
+ if( R(2,1)>0 ) res(0)=M_PI/2; else res(0) = -M_PI/2;
+ res(2) = atan2( -R(0,2),R(0,0) );
+ res(1) = 0;
+
+ /* To sum up: if X=PI/2, then Y and Z are in singularity.
+ * we cannot decide both of then. I fixed Y=0, which
+ * means that all the measurement coming from the sensor
+ * is assumed to be drift of the gyro. */
+ }
+ else
+ {
+ double &X = res(0);
+ double &Y = res(1);
+ double &Z = res(2);
+
+ Y = atan2( R(2,0),R(2,2) );
+ Z = atan2( R(0,1),R(1,1) );
+ if( fabs(R(2,0))>fabs(R(2,2)) )
+ { X=atan2( R(2,1)*sin(Y),R(2,0) ); }
+ else
+ { X=atan2( R(2,1)*cos(Y),R(2,2) ); }
+ }
+
+ sotDEBUG(35) << "angles = " << res;
+
+
+ sotDEBUGOUT(15);
+ return res;
+}
+
+/* compute the transformation matrix of the flexibility, ie
+ * feetRleg.
+ */
+MatrixRotation& AngleEstimator::
+computeFlexibilityFromAngles( MatrixRotation& res,
+ const int& time )
+{
+ sotDEBUGIN(15);
+
+ const ml::Vector & angles = anglesSOUT( time );
+ double cx= cos( angles(0) );
+ double sx= sin( angles(0) );
+ double cy= cos( angles(1) );
+ double sy= sin( angles(1) );
+
+ res(0,0) = cy;
+ res(0,1) = 0;
+ res(0,2) = -sy;
+
+ res(1,0) = -sx*sy;
+ res(1,1) = cx;
+ res(1,2) = -sx*cy;
+
+ res(2,0) = cx*sy;
+ res(2,1) = sx;
+ res(2,2) = cx*cy;
+
+ sotDEBUGOUT(15);
+ return res;
+}
+
+/* Compute the rotation matrix of the drift, ie the
+ * transfo from the world frame to the estimated (drifted) world
+ * frame: worldRworldest.
+ */
+MatrixRotation& AngleEstimator::
+computeDriftFromAngles( MatrixRotation& res,
+ const int& time )
+{
+ sotDEBUGIN(15);
+
+ const ml::Vector & angles = anglesSOUT( time );
+ double cz = cos( angles(2) );
+ double sz = sin( angles(2) );
+
+ res(0,0) = cz;
+ res(0,1) = -sz;
+ res(0,2) = 0;
+
+ // z is the positive angle (R_{-z} has been computed
+ // in the <angles> function). Thus, the /-/sin(z) is in 0,1
+ res(1,0) = sz;
+ res(1,1) = cz;
+ res(1,2) = 0;
+
+ res(2,0) = 0;
+ res(2,1) = 0;
+ res(2,2) = 1;
+
+ sotDEBUGOUT(15);
+ return res;
+}
+
+MatrixRotation& AngleEstimator::
+computeSensorWorldRotation( MatrixRotation& res,
+ const int& time )
+{
+ sotDEBUGIN(15);
+
+ const MatrixRotation & worldRworldest = driftSOUT( time );
+ const MatrixRotation & worldestRsensor = sensorWorldRotationSIN( time );
+
+ worldRworldest.multiply( worldestRsensor,res );
+
+ sotDEBUGOUT(15);
+ return res;
+}
+
+MatrixRotation& AngleEstimator::
+computeWaistWorldRotation( MatrixRotation& res,
+ const int& time )
+{
+ sotDEBUGIN(15);
+
+ // chest = sensor
+ const MatrixRotation & worldRsensor = sensorWorldRotationSOUT( time );
+ const MatrixHomogeneous & waistMchest = sensorEmbeddedPositionSIN( time );
+ MatrixRotation waistRchest; waistMchest.extract( waistRchest );
+
+ worldRsensor .multiply( waistRchest.transpose(),res );
+
+ sotDEBUGOUT(15);
+ return res;
+}
+
+
+
+MatrixHomogeneous& AngleEstimator::
+computeWaistWorldPosition( MatrixHomogeneous& res,
+ const int& time )
+{
+ sotDEBUGIN(15);
+
+ const MatrixHomogeneous & waistMleg = contactEmbeddedPositionSIN( time );
+ MatrixHomogeneous legMwaist; waistMleg.inverse( legMwaist );
+
+ if( fromSensor )
+ {
+ const MatrixRotation & Rflex = flexibilitySOUT( time ); // footRleg
+ ml::Vector zero(3); zero.fill(0.);
+ MatrixHomogeneous footMleg; footMleg.buildFrom( Rflex,zero );
+
+ footMleg.multiply( legMwaist,res );
+ }
+ else { res = legMwaist; }
+
+ sotDEBUGOUT(15);
+ return res;
+}
+
+ml::Vector& AngleEstimator::
+computeWaistWorldPoseRPY( ml::Vector& res,
+ const int& time )
+{
+ const MatrixHomogeneous & M = waistWorldPositionSOUT( time );
+
+ MatrixRotation R; M.extract(R);
+ VectorRollPitchYaw r; r.fromMatrix(R);
+ ml::Vector t(3); M.extract(t);
+
+ res.resize(6);
+ for( int i=0;i<3;++i ) { res(i)=t(i); res(i+3)=r(i); }
+
+ return res;
+}
+
+/* --- PARAMS --------------------------------------------------------------- */
+/* --- PARAMS --------------------------------------------------------------- */
+/* --- PARAMS --------------------------------------------------------------- */
+void AngleEstimator::
+commandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os )
+{
+ sotDEBUG(25) << "Cmd " << cmdLine <<std::endl;
+
+ if( cmdLine == "help" )
+ {
+ Entity::commandLine(cmdLine,cmdArgs,os);
+ }
+ else if( cmdLine == "fromSensor" )
+ {
+ std::string val; cmdArgs>>val;
+ if( ("true"==val)||("false"==val) )
+ {
+ fromSensor = ( val=="true" );
+ } else {
+ os << "fromSensor = " << (fromSensor?"true":"false") << std::endl;
+ }
+ }
+ else { Entity::commandLine( cmdLine,cmdArgs,os); }
+}
+
diff --git a/src/dynamic-hrp2.cpp b/src/dynamic-hrp2.cpp
new file mode 100644
index 0000000..276b42f
--- /dev/null
+++ b/src/dynamic-hrp2.cpp
@@ -0,0 +1,67 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: Dynamic.h
+ * Project: SOT
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+#include <MatrixAbstractLayer/MatrixAbstractLayer.h>
+
+#include <sot-dynamic/dynamic-hrp2.h>
+#include <sot-core/debug.h>
+
+#include <robotDynamics/jrlRobotDynamicsObjectConstructor.h>
+
+#include <dynamic-graph/factory.h>
+
+using namespace sot;
+using namespace dynamicgraph;
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(DynamicHrp2,"DynamicHrp2");
+
+DynamicHrp2::
+DynamicHrp2( const std::string & name )
+ :Dynamic(name,false)
+{
+ sotDEBUGIN(15);
+ DynamicHrp2::buildModelHrp2();
+ sotDEBUGOUT(15);
+}
+
+void DynamicHrp2::
+buildModelHrp2( void )
+{
+ sotDEBUGIN(15);
+ dynamicsJRLJapan::ObjectFactory aRobotDynamicsObjectConstructor;
+
+ Chrp2OptHumanoidDynamicRobot * aHDR = new Chrp2OptHumanoidDynamicRobot(&aRobotDynamicsObjectConstructor);
+
+ m_HDR = aHDR;
+
+ if (aHDR==0)
+ {
+ std::cerr<< "Dynamic cast on HDR failed " << std::endl;
+ exit(-1);
+ }
+
+ sotDEBUGOUT(15);
+}
+
+
+DynamicHrp2::
+~DynamicHrp2( void )
+{
+ sotDEBUGINOUT(5);
+ return;
+}
+
diff --git a/src/dynamic.cpp b/src/dynamic.cpp
new file mode 100644
index 0000000..137373f
--- /dev/null
+++ b/src/dynamic.cpp
@@ -0,0 +1,1018 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, 2007-2009
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: Dynamic.h
+ * Project: SOT
+ * Author: Nicolas Mansard, Olivier Stasse
+ *
+ * Description
+ * ============
+ *
+ * \file This object provides geometric, kinematic and dynamic
+ * information on a robot described through the dynamicsJRLJapan library.
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+#include <MatrixAbstractLayer/MatrixAbstractLayer.h>
+
+#include <sot-dynamic/dynamic.h>
+#include <sot-core/debug.h>
+
+#include <robotDynamics/jrlHumanoidDynamicRobot.h>
+#include <robotDynamics/jrlRobotDynamicsObjectConstructor.h>
+
+#include <dynamic-graph/factory.h>
+
+using namespace sot;
+using namespace dynamicgraph;
+
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Dynamic,"Dynamic");
+
+using namespace std;
+
+Dynamic::
+Dynamic( const std::string & name, bool build )
+ :Entity(name)
+ ,m_HDR(NULL)
+
+ ,vrmlDirectory()
+ ,vrmlMainFile()
+ ,xmlSpecificityFile()
+ ,xmlRankFile()
+
+ ,init(false)
+
+ ,jointPositionSIN(NULL,"sotDynamic("+name+")::input(vector)::position")
+ ,freeFlyerPositionSIN(NULL,"sotDynamic("+name+")::input(vector)::ffposition")
+ ,jointVelocitySIN(NULL,"sotDynamic("+name+")::input(vector)::velocity")
+ ,freeFlyerVelocitySIN(NULL,"sotDynamic("+name+")::input(vector)::ffvelocity")
+ ,jointAccelerationSIN(NULL,"sotDynamic("+name+")::input(vector)::acceleration")
+ ,freeFlyerAccelerationSIN(NULL,"sotDynamic("+name+")::input(vector)::ffacceleration")
+
+ ,firstSINTERN( boost::bind(&Dynamic::initNewtonEuler,this,_1,_2),
+ sotNOSIGNAL,"sotDynamic("+name+")::intern(dummy)::init" )
+ ,newtonEulerSINTERN( boost::bind(&Dynamic::computeNewtonEuler,this,_1,_2),
+ firstSINTERN<<jointPositionSIN<<freeFlyerPositionSIN
+ <<jointVelocitySIN<<freeFlyerVelocitySIN
+ <<jointAccelerationSIN<<freeFlyerAccelerationSIN,
+ "sotDynamic("+name+")::intern(dummy)::newtoneuleur" )
+
+ ,zmpSOUT( boost::bind(&Dynamic::computeZmp,this,_1,_2),
+ newtonEulerSINTERN,
+ "sotDynamic("+name+")::output(vector)::zmp" )
+ ,JcomSOUT( boost::bind(&Dynamic::computeJcom,this,_1,_2),
+ newtonEulerSINTERN,
+ "sotDynamic("+name+")::output(matrix)::Jcom" )
+ ,comSOUT( boost::bind(&Dynamic::computeCom,this,_1,_2),
+ newtonEulerSINTERN,
+ "sotDynamic("+name+")::output(vector)::com" )
+ ,inertiaSOUT( boost::bind(&Dynamic::computeInertia,this,_1,_2),
+ newtonEulerSINTERN,
+ "sotDynamic("+name+")::output(matrix)::inertia" )
+ ,footHeightSOUT( boost::bind(&Dynamic::computeFootHeight,this,_1,_2),
+ newtonEulerSINTERN,
+ "sotDynamic("+name+")::output(double)::footHeight" )
+
+ ,upperJlSOUT( boost::bind(&Dynamic::getUpperJointLimits,this,_1,_2),
+ sotNOSIGNAL,
+ "sotDynamic("+name+")::output(vector)::upperJl" )
+
+ ,lowerJlSOUT( boost::bind(&Dynamic::getLowerJointLimits,this,_1,_2),
+ sotNOSIGNAL,
+ "sotDynamic("+name+")::output(vector)::lowerJl" )
+
+ ,inertiaRotorSOUT( "sotDynamic("+name+")::output(matrix)::inertiaRotor" )
+ ,gearRatioSOUT( "sotDynamic("+name+")::output(matrix)::gearRatio" )
+ ,inertiaRealSOUT( boost::bind(&Dynamic::computeInertiaReal,this,_1,_2),
+ inertiaSOUT << gearRatioSOUT << inertiaRotorSOUT,
+ "sotDynamic("+name+")::output(matrix)::inertiaReal" )
+ ,MomentaSOUT( boost::bind(&Dynamic::computeMomenta,this,_1,_2),
+ newtonEulerSINTERN,
+ "sotDynamic("+name+")::output(vector)::momenta" )
+ ,AngularMomentumSOUT( boost::bind(&Dynamic::computeAngularMomentum,this,_1,_2),
+ newtonEulerSINTERN,
+ "sotDynamic("+name+")::output(vector)::angularmomentum" )
+{
+ sotDEBUGIN(5);
+
+ if( build ) buildModel();
+
+ firstSINTERN.setDependencyType(TimeDependency<int>::BOOL_DEPENDENT);
+ //DEBUG: Why =0? should be function. firstSINTERN.setConstant(0);
+
+ signalRegistration( jointPositionSIN<<freeFlyerPositionSIN
+ <<jointVelocitySIN<<freeFlyerVelocitySIN
+ <<jointAccelerationSIN<<freeFlyerAccelerationSIN);
+ signalRegistration( zmpSOUT<<comSOUT<<JcomSOUT<<footHeightSOUT
+ <<upperJlSOUT<<lowerJlSOUT<<inertiaSOUT
+ <<inertiaRealSOUT << inertiaRotorSOUT << gearRatioSOUT );
+ signalRegistration( MomentaSOUT << AngularMomentumSOUT );
+
+ sotDEBUGOUT(5);
+}
+
+void Dynamic::
+buildModel( void )
+{
+ sotDEBUGIN(5);
+
+ djj::ObjectFactory aRobotDynamicsObjectConstructor;
+
+ m_HDR = aRobotDynamicsObjectConstructor.createHumanoidDynamicRobot();
+
+ sotDEBUGOUT(5);
+}
+
+
+Dynamic::
+~Dynamic( void )
+{
+ sotDEBUGIN(5);
+ if( 0!=m_HDR )
+ {
+ delete m_HDR;
+ m_HDR = 0;
+ }
+
+ for( std::list< SignalBase<int>* >::iterator iter = genericSignalRefs.begin();
+ iter != genericSignalRefs.end();
+ ++iter )
+ {
+ SignalBase<int>* sigPtr = *iter;
+ delete sigPtr;
+ }
+
+ sotDEBUGOUT(5);
+ return;
+}
+
+/* --- CONFIG --------------------------------------------------------------- */
+/* --- CONFIG --------------------------------------------------------------- */
+/* --- CONFIG --------------------------------------------------------------- */
+/* --- CONFIG --------------------------------------------------------------- */
+void Dynamic::
+setVrmlDirectory( const std::string& filename )
+{
+ vrmlDirectory = filename;
+}
+void Dynamic::
+setVrmlMainFile( const std::string& filename )
+{
+ vrmlMainFile = filename;
+}
+void Dynamic::
+setXmlSpecificityFile( const std::string& filename )
+{
+ xmlSpecificityFile = filename;
+}
+void Dynamic::
+setXmlRankFile( const std::string& filename )
+{
+ xmlRankFile = filename;
+}
+void Dynamic::
+parseConfigFiles( void )
+{
+ sotDEBUGIN(15);
+ try
+ {
+ sotDEBUG(35) << "Parse the vrml."<<endl;
+ string RobotFileName = vrmlDirectory+vrmlMainFile;
+ djj::parseOpenHRPVRMLFile(*m_HDR,RobotFileName,xmlRankFile,xmlSpecificityFile);
+ }
+ catch( ... )
+ { SOT_THROW ExceptionDynamic( ExceptionDynamic::DYNAMIC_JRL,
+ "Error while parsing." ); }
+ //inertia.init( aHDMB );
+ init = true;
+ sotDEBUGOUT(15);
+}
+
+/* --- SIGNAL ACTIVATION ---------------------------------------------------- */
+/* --- SIGNAL ACTIVATION ---------------------------------------------------- */
+/* --- SIGNAL ACTIVATION ---------------------------------------------------- */
+dg::SignalTimeDependent< ml::Matrix,int > & Dynamic::
+createJacobianSignal( const std::string& signame,const unsigned int& bodyRank )
+{
+
+ vector<CjrlJoint *> aVec = m_HDR->jointVector();
+ if( bodyRank>=aVec.size() )
+ {
+ SOT_THROW ExceptionDynamic( ExceptionDynamic::JOINT_RANK,
+ "Joint rank is too high.",
+ "(rank=%d, while creating J signal).",
+ bodyRank );
+ }
+ CjrlJoint * aJoint = aVec[bodyRank];
+
+
+ dg::SignalTimeDependent< ml::Matrix,int > * sig
+ = new dg::SignalTimeDependent< ml::Matrix,int >
+ ( boost::bind(&Dynamic::computeGenericJacobian,this,aJoint,_1,_2),
+ newtonEulerSINTERN,
+ "sotDynamic("+name+")::output(matrix)::"+signame );
+
+ genericSignalRefs.push_back( sig );
+ signalRegistration( *sig );
+ return *sig;
+}
+
+dg::SignalTimeDependent< ml::Matrix,int > & Dynamic::
+createEndeffJacobianSignal( const std::string& signame,const unsigned int& bodyRank )
+{
+ sotDEBUGIN(15);
+
+ vector<CjrlJoint *> aVec = m_HDR->jointVector();
+ if( bodyRank>=aVec.size() )
+ {
+ SOT_THROW ExceptionDynamic( ExceptionDynamic::JOINT_RANK,
+ "Joint rank is too high.",
+ "(rank=%d, while creating J signal).",
+ bodyRank );
+ }
+ CjrlJoint * aJoint = aVec[bodyRank];
+
+
+ dg::SignalTimeDependent< ml::Matrix,int > * sig
+ = new dg::SignalTimeDependent< ml::Matrix,int >
+ ( boost::bind(&Dynamic::computeGenericEndeffJacobian,this,aJoint,_1,_2),
+ newtonEulerSINTERN,
+ "sotDynamic("+name+")::output(matrix)::"+signame );
+
+ genericSignalRefs.push_back( sig );
+ signalRegistration( *sig );
+
+ sotDEBUGOUT(15);
+ return *sig;
+}
+
+void Dynamic::
+destroyJacobianSignal( const std::string& signame )
+{
+ bool deletable = false;
+ dg::SignalTimeDependent< ml::Matrix,int > * sig = & jacobiansSOUT( signame );
+ for( std::list< SignalBase<int>* >::iterator iter = genericSignalRefs.begin();
+ iter != genericSignalRefs.end();
+ ++iter )
+ {
+ if( (*iter) == sig ) { genericSignalRefs.erase(iter); deletable = true; break; }
+ }
+
+ if(! deletable )
+ {
+ SOT_THROW ExceptionDynamic( ExceptionDynamic::CANT_DESTROY_SIGNAL,
+ "Cannot destroy signal",
+ " (while trying to remove generic jac. signal <%s>).",
+ signame.c_str() );
+ }
+
+ signalDeregistration( signame );
+
+ delete sig;
+}
+
+/* --- POINT --- */
+/* --- POINT --- */
+/* --- POINT --- */
+
+dg::SignalTimeDependent< MatrixHomogeneous,int >& Dynamic::
+createPositionSignal( const std::string& signame,const unsigned int& bodyRank )
+{
+ sotDEBUGIN(15);
+
+ vector<CjrlJoint *> aVec = m_HDR->jointVector();
+
+ if( bodyRank>=aVec.size() )
+ {
+ SOT_THROW ExceptionDynamic( ExceptionDynamic::JOINT_RANK,
+ "Joint rank is too high.",
+ "(rank=%d, while creating position signal).",
+ bodyRank );
+ }
+ CjrlJoint * aJoint = aVec[bodyRank];
+
+ dg::SignalTimeDependent< MatrixHomogeneous,int > * sig
+ = new dg::SignalTimeDependent< MatrixHomogeneous,int >
+ ( boost::bind(&Dynamic::computeGenericPosition,this,aJoint,_1,_2),
+ newtonEulerSINTERN,
+ "sotDynamic("+name+")::output(matrixHomo)::"+signame );
+
+ genericSignalRefs.push_back( sig );
+ signalRegistration( *sig );
+
+ sotDEBUGOUT(15);
+ return *sig;
+}
+
+
+void Dynamic::
+destroyPositionSignal( const std::string& signame )
+{
+ bool deletable = false;
+ dg::SignalTimeDependent< MatrixHomogeneous,int > * sig = & positionsSOUT( signame );
+ for( std::list< SignalBase<int>* >::iterator iter = genericSignalRefs.begin();
+ iter != genericSignalRefs.end();
+ ++iter )
+ {
+ if( (*iter) == sig ) { genericSignalRefs.erase(iter); deletable = true; break; }
+ }
+
+ if(! deletable )
+ {
+ SOT_THROW ExceptionDynamic( ExceptionDynamic::CANT_DESTROY_SIGNAL,
+ "Cannot destroy signal",
+ " (while trying to remove generic pos. signal <%s>).",
+ signame.c_str() );
+ }
+
+ signalDeregistration( signame );
+
+ delete sig;
+}
+
+/* --- ACCELERATION --- */
+/* --- ACCELERATION --- */
+/* --- ACCELERATION --- */
+
+dg::SignalTimeDependent< ml::Vector,int >& Dynamic::
+createAccelerationSignal( const std::string& signame,const unsigned int& bodyRank )
+{
+ sotDEBUGIN(15);
+
+ vector<CjrlJoint *> aVec = m_HDR->jointVector();
+
+ if( bodyRank>=aVec.size() )
+ {
+ SOT_THROW ExceptionDynamic( ExceptionDynamic::JOINT_RANK,
+ "Joint rank is too high.",
+ "(rank=%d, while creating acceleration signal).",
+ bodyRank );
+ }
+ CjrlJoint * aJoint = aVec[bodyRank];
+
+ dg::SignalTimeDependent< ml::Vector,int > * sig
+ = new dg::SignalTimeDependent< ml::Vector,int >
+ ( boost::bind(&Dynamic::computeGenericAcceleration,this,aJoint,_1,_2),
+ newtonEulerSINTERN,
+ "sotDynamic("+name+")::output(matrixHomo)::"+signame );
+
+ genericSignalRefs.push_back( sig );
+ signalRegistration( *sig );
+
+ sotDEBUGOUT(15);
+ return *sig;
+}
+
+
+void Dynamic::
+destroyAccelerationSignal( const std::string& signame )
+{
+ bool deletable = false;
+ dg::SignalTimeDependent< ml::Vector,int > * sig = & accelerationsSOUT( signame );
+ for( std::list< SignalBase<int>* >::iterator iter = genericSignalRefs.begin();
+ iter != genericSignalRefs.end();
+ ++iter )
+ {
+ if( (*iter) == sig ) { genericSignalRefs.erase(iter); deletable = true; break; }
+ }
+
+ if(! deletable )
+ {
+ SOT_THROW ExceptionDynamic( ExceptionDynamic::CANT_DESTROY_SIGNAL,
+ "Cannot destroy signal",
+ " (while trying to remove generic acc signal <%s>).",
+ signame.c_str() );
+ }
+
+ signalDeregistration( signame );
+
+ delete sig;
+}
+/* --- COMPUTE -------------------------------------------------------------- */
+/* --- COMPUTE -------------------------------------------------------------- */
+/* --- COMPUTE -------------------------------------------------------------- */
+
+#include <MatrixAbstractLayer/boostspecific.h>
+
+static void MAAL1_V3d_to_MAAL2( const vector3d& source,
+ ml::Vector & res )
+{
+ sotDEBUG(5) << source <<endl;
+ res(0) = source[0];
+ res(1) = source[1];
+ res(2) = source[2];
+}
+
+ml::Matrix& Dynamic::
+computeGenericJacobian( CjrlJoint * aJoint,ml::Matrix& res,int time )
+{
+ sotDEBUGIN(25);
+ newtonEulerSINTERN(time);
+
+ aJoint->computeJacobianJointWrtConfig();
+ res.initFromMotherLib(aJoint->jacobianJointWrtConfig());
+ sotDEBUGOUT(25);
+
+ return res;
+}
+
+ml::Matrix& Dynamic::
+computeGenericEndeffJacobian( CjrlJoint * aJoint,ml::Matrix& res,int time )
+{
+ sotDEBUGIN(25);
+ newtonEulerSINTERN(time);
+
+ aJoint->computeJacobianJointWrtConfig();
+
+ ml::Matrix J,V(6,6);
+ J.initFromMotherLib(aJoint->jacobianJointWrtConfig());
+
+ /* --- TODO --- */
+ MatrixHomogeneous M;
+ computeGenericPosition(aJoint,M,time);
+ //M=M.inverse();
+
+ for( int i=0;i<3;++i )
+ for( int j=0;j<3;++j )
+ {
+ V(i,j)=M(j,i);
+ V(i+3,j+3)=M(j,i);
+ V(i+3,j)=0.;
+ V(i,j+3)=0.;
+ }
+
+ sotDEBUG(25) << "0Jn = "<< J;
+ sotDEBUG(25) << "V = "<< V;
+ V.multiply(J,res);
+ sotDEBUGOUT(25);
+
+ return res;
+}
+
+MatrixHomogeneous& Dynamic::
+computeGenericPosition( CjrlJoint * aJoint,MatrixHomogeneous& res,int time )
+{
+ sotDEBUGIN(25);
+ newtonEulerSINTERN(time);
+ const matrix4d & m4 = aJoint->currentTransformation();
+
+ res.resize(4,4);
+ for( int i=0;i<4;++i )
+ for( int j=0;j<4;++j )
+ res(i,j) = MAL_S4x4_MATRIX_ACCESS_I_J(m4,i,j);
+
+ // aJoint->computeJacobianJointWrtConfig();
+ //res.initFromMotherLib(aJoint->jacobianJointWrtConfig());
+
+ // adaptation to the new dynamic -- to be optimized
+ matrix4d initialTr;
+ initialTr = aJoint->initialPosition();
+ MAL_S4x4_MATRIX_ACCESS_I_J(initialTr,0,3) = 0.0;
+ MAL_S4x4_MATRIX_ACCESS_I_J(initialTr,1,3) = 0.0;
+ MAL_S4x4_MATRIX_ACCESS_I_J(initialTr,2,3) = 0.0;
+
+ matrix4d invrot;
+ for(unsigned int i=0;i<3;i++)
+ for(unsigned int j=0;j<3;j++)
+ {
+ MAL_S4x4_MATRIX_ACCESS_I_J(invrot,i,j)=0.0;
+ for(unsigned int k=0;k<3;k++)
+ {
+ MAL_S4x4_MATRIX_ACCESS_I_J(invrot,i,j)+=
+ MAL_S4x4_MATRIX_ACCESS_I_J(res,i,k) *
+ MAL_S4x4_MATRIX_ACCESS_I_J(initialTr,j,k);
+ }
+ }
+ for(unsigned int i=0;i<3;i++)
+ for(unsigned int j=0;j<3;j++)
+ MAL_S4x4_MATRIX_ACCESS_I_J(res,i,j) =
+ MAL_S4x4_MATRIX_ACCESS_I_J(invrot,i,j);
+ //end of the adaptation
+
+
+ sotDEBUGOUT(25);
+ return res;
+}
+
+ml::Vector& Dynamic::
+computeGenericAcceleration( CjrlJoint* j,ml::Vector& res,int time )
+{
+ sotDEBUGIN(25);
+ newtonEulerSINTERN(time);
+ CjrlRigidAcceleration aRA = j->jointAcceleration();
+ vector3d al= aRA.linearAcceleration();
+ vector3d ar= aRA.rotationAcceleration(); // TODO: Dont copy, reference.
+
+ res.resize(6);
+ for( int i=0;i<3;++i )
+ {
+ res(i)=al(i);
+ res(i+3)=ar(i);
+ }
+
+ sotDEBUGOUT(25);
+ return res;
+}
+
+
+
+ml::Vector& Dynamic::
+computeZmp( ml::Vector& ZMPval,int time )
+{
+ if (ZMPval.size()!=3)
+ ZMPval.resize(3);
+
+ newtonEulerSINTERN(time);
+ MAAL1_V3d_to_MAAL2(m_HDR->zeroMomentumPoint(),ZMPval);
+ sotDEBUGOUT(25);
+ return ZMPval;
+}
+
+
+ml::Vector& Dynamic::
+computeMomenta(ml::Vector & Momenta, int time)
+{
+ vector3d LinearMomentum, AngularMomentum;
+
+ if (Momenta.size()!=6)
+ Momenta.resize(6);
+
+ sotDEBUGIN(25);
+ newtonEulerSINTERN(time);
+ LinearMomentum = m_HDR->linearMomentumRobot();
+ AngularMomentum = m_HDR->angularMomentumRobot();
+
+ for(unsigned int i=0;i<3;i++)
+ {
+ Momenta(i) = LinearMomentum(i);
+ Momenta(i+3) = AngularMomentum(i);
+ }
+
+ sotDEBUGOUT(25) << "Momenta :" << Momenta ;
+ return Momenta;
+}
+
+ml::Vector& Dynamic::
+computeAngularMomentum(ml::Vector & Momenta, int time)
+{
+ vector3d AngularMomentum;
+
+ if (Momenta.size()!=3)
+ Momenta.resize(3);
+
+ sotDEBUGIN(25);
+ newtonEulerSINTERN(time);
+ AngularMomentum = m_HDR->angularMomentumRobot();
+
+ for(unsigned int i=0;i<3;i++)
+ {
+ Momenta(i) = AngularMomentum(i);
+ }
+
+ sotDEBUGOUT(25) << "AngularMomenta :" << Momenta ;
+ return Momenta;
+
+}
+
+ml::Matrix& Dynamic::
+computeJcom( ml::Matrix& Jcom,int time )
+{
+ sotDEBUGIN(25);
+ newtonEulerSINTERN(time);
+ m_HDR->computeJacobianCenterOfMass();
+ Jcom.initFromMotherLib(m_HDR->jacobianCenterOfMass());
+ sotDEBUGOUT(25);
+ return Jcom;
+}
+
+ml::Vector& Dynamic::
+computeCom( ml::Vector& com,int time )
+{
+ sotDEBUGIN(25);
+ newtonEulerSINTERN(time);
+ com.resize(3);
+ MAAL1_V3d_to_MAAL2(m_HDR->positionCenterOfMass(),com);
+ sotDEBUGOUT(25);
+ return com;
+}
+
+ml::Matrix& Dynamic::
+computeInertia( ml::Matrix& A,int time )
+{
+ sotDEBUGIN(25);
+ newtonEulerSINTERN(time);
+
+ m_HDR->computeInertiaMatrix();
+ A.initFromMotherLib(m_HDR->inertiaMatrix());
+
+ if( 1==debugInertia )
+ {
+ for( unsigned int i=0;i<18;++i )
+ for( unsigned int j=0;j<36;++j )
+ if( i==j ) A(i,i)=1;
+ else { A(i,j)=A(j,i)=0; }
+ for( unsigned int i=20;i<22;++i )
+ for( unsigned int j=0;j<36;++j )
+ if( i==j ) A(i,i)=1;
+ else { A(i,j)=A(j,i)=0; }
+ for( unsigned int i=28;i<36;++i )
+ for( unsigned int j=0;j<36;++j )
+ if( i==j ) A(i,i)=1;
+ else { A(i,j)=A(j,i)=0; }
+ }
+ else if( 2==debugInertia )
+ {
+ for( unsigned int i=0;i<18;++i )
+ for( unsigned int j=0;j<36;++j )
+ if( i==j ) A(i,i)=1;
+ else { A(i,j)=A(j,i)=0; }
+ for( unsigned int i=20;i<22;++i )
+ for( unsigned int j=0;j<36;++j )
+ if( i==j ) A(i,i)=1;
+ else { A(i,j)=A(j,i)=0; }
+ for( unsigned int i=28;i<29;++i )
+ for( unsigned int j=0;j<36;++j )
+ if( i==j ) A(i,i)=1;
+ else { A(i,j)=A(j,i)=0; }
+ for( unsigned int i=35;i<36;++i )
+ for( unsigned int j=0;j<36;++j )
+ if( i==j ) A(i,i)=1;
+ else { A(i,j)=A(j,i)=0; }
+ }
+
+ sotDEBUGOUT(25);
+ return A;
+}
+
+ml::Matrix& Dynamic::
+computeInertiaReal( ml::Matrix& res,int time )
+{
+ sotDEBUGIN(25);
+
+ const ml::Matrix & A = inertiaSOUT(time);
+ const ml::Vector & gearRatio = gearRatioSOUT(time);
+ const ml::Vector & inertiaRotor = inertiaRotorSOUT(time);
+
+ res = A;
+ for( unsigned int i=0;i<gearRatio.size();++i )
+ res(i,i) += (gearRatio(i)*gearRatio(i)*inertiaRotor(i));
+
+ sotDEBUGOUT(25);
+ return res;
+}
+
+double& Dynamic::
+computeFootHeight( double& foot,int time )
+{
+ sotDEBUGIN(25);
+ newtonEulerSINTERN(time);
+ //foot=m_HDR->footHeight();
+ CjrlFoot *RightFoot = m_HDR->rightFoot();
+ vector3d AnkleInLocalRefFrame,SoleCenterInLocalRefFrame;
+ RightFoot->getAnklePositionInLocalFrame(AnkleInLocalRefFrame);
+ RightFoot->getSoleCenterInLocalFrame(SoleCenterInLocalRefFrame);
+ foot = fabs(AnkleInLocalRefFrame[2]-SoleCenterInLocalRefFrame[2]);
+ sotDEBUGOUT(25);
+ return foot;
+}
+
+
+/* --- SIGNAL --------------------------------------------------------------- */
+/* --- SIGNAL --------------------------------------------------------------- */
+/* --- SIGNAL --------------------------------------------------------------- */
+
+dg::SignalTimeDependent<ml::Matrix,int>& Dynamic::
+jacobiansSOUT( const std::string& name )
+{
+ SignalBase<int> & sigabs = Entity::getSignal(name);
+
+ try {
+ dg::SignalTimeDependent<ml::Matrix,int>& res
+ = dynamic_cast< dg::SignalTimeDependent<ml::Matrix,int>& >( sigabs );
+ return res;
+ } catch( std::bad_cast e ) {
+ SOT_THROW ExceptionSignal( ExceptionSignal::BAD_CAST,
+ "Impossible cast.",
+ " (while getting signal <%s> of type matrix.",
+ name.c_str());
+ }
+}
+dg::SignalTimeDependent<MatrixHomogeneous,int>& Dynamic::
+positionsSOUT( const std::string& name )
+{
+ SignalBase<int> & sigabs = Entity::getSignal(name);
+
+ try {
+ dg::SignalTimeDependent<MatrixHomogeneous,int>& res
+ = dynamic_cast< dg::SignalTimeDependent<MatrixHomogeneous,int>& >( sigabs );
+ return res;
+ } catch( std::bad_cast e ) {
+ SOT_THROW ExceptionSignal( ExceptionSignal::BAD_CAST,
+ "Impossible cast.",
+ " (while getting signal <%s> of type matrixHomo.",
+ name.c_str());
+ }
+}
+dg::SignalTimeDependent<ml::Vector,int>& Dynamic::
+accelerationsSOUT( const std::string& name )
+{
+ SignalBase<int> & sigabs = Entity::getSignal(name);
+
+ try {
+ dg::SignalTimeDependent<ml::Vector,int>& res
+ = dynamic_cast< dg::SignalTimeDependent<ml::Vector,int>& >( sigabs );
+ return res;
+ } catch( std::bad_cast e ) {
+ SOT_THROW ExceptionSignal( ExceptionSignal::BAD_CAST,
+ "Impossible cast.",
+ " (while getting signal <%s> of type Vector.",
+ name.c_str());
+ }
+}
+
+
+int& Dynamic::
+computeNewtonEuler( int& dummy,int time )
+{
+ sotDEBUGIN(15);
+ ml::Vector pos = jointPositionSIN(time); // TODO &pos
+ ml::Vector vel = jointVelocitySIN(time);
+ ml::Vector acc = jointAccelerationSIN(time);
+
+ firstSINTERN(time);
+ if( freeFlyerPositionSIN )
+ {
+ const ml::Vector& ffpos = freeFlyerPositionSIN(time);
+ for( int i=0;i<6;++i ) pos(i) = ffpos(i);
+ }
+ if( freeFlyerVelocitySIN )
+ {
+ const ml::Vector& ffvel = freeFlyerVelocitySIN(time);
+ for( int i=0;i<6;++i ) vel(i) = ffvel(i);
+ }
+ if( freeFlyerAccelerationSIN )
+ {
+ const ml::Vector& ffacc = freeFlyerAccelerationSIN(time);
+ for( int i=0;i<6;++i ) acc(i) = ffacc(i);
+ }
+ if(! m_HDR->currentConfiguration(pos.accessToMotherLib()))
+ {
+ SOT_THROW ExceptionDynamic( ExceptionDynamic::JOINT_SIZE,
+ "Position vector size uncorrect",
+ " (Vector size is %d, should be %d).",
+ pos.size(),m_HDR->currentConfiguration().size() );
+ }
+
+
+ if(! m_HDR->currentVelocity(vel.accessToMotherLib()) )
+ {
+ SOT_THROW ExceptionDynamic( ExceptionDynamic::JOINT_SIZE,
+ "Velocity vector size uncorrect",
+ " (Vector size is %d, should be %d).",
+ vel.size(),m_HDR->currentVelocity().size() );
+ }
+
+ if(! m_HDR->currentAcceleration(acc.accessToMotherLib()) )
+ {
+ SOT_THROW ExceptionDynamic( ExceptionDynamic::JOINT_SIZE,
+ "Acceleration vector size uncorrect",
+ " (Vector size is %d, should be %d).",
+ acc.size(),m_HDR->currentAcceleration().size() );
+ }
+
+ m_HDR->computeForwardKinematics();
+
+ sotDEBUG(1)<< "pos = " <<pos <<endl;
+ sotDEBUG(1)<< "vel = " <<vel <<endl;
+ sotDEBUG(1)<< "acc = " <<acc <<endl;
+
+ sotDEBUGOUT(15);
+ return dummy;
+}
+int& Dynamic::
+initNewtonEuler( int& dummy,int time )
+{
+ sotDEBUGIN(15);
+ firstSINTERN.setReady(false);
+ computeNewtonEuler(dummy,time);
+ for( int i=0;i<3;++i )
+ m_HDR->computeForwardKinematics();
+
+ sotDEBUGOUT(15);
+ return dummy;
+}
+
+ml::Vector& Dynamic::
+getUpperJointLimits( ml::Vector& res,const int& time )
+{
+ sotDEBUGIN(25);
+ const unsigned int NBJ = m_HDR->numberDof();
+ res.resize( NBJ );
+ for( unsigned int i=0;i<NBJ;++i )
+ {
+ res(i)=m_HDR->upperBoundDof( i );
+ }
+ sotDEBUGOUT(25);
+ return res;
+}
+
+ml::Vector& Dynamic::
+getLowerJointLimits( ml::Vector& res,const int& time )
+{
+ sotDEBUGIN(25);
+ const unsigned int NBJ = m_HDR->numberDof();
+ res.resize( NBJ );
+ for( unsigned int i=0;i<NBJ;++i )
+ {
+ res(i)=m_HDR->lowerBoundDof( i );
+ }
+ sotDEBUGOUT(25);
+ return res;
+}
+
+
+
+/* --- PARAMS --------------------------------------------------------------- */
+/* --- PARAMS --------------------------------------------------------------- */
+/* --- PARAMS --------------------------------------------------------------- */
+void Dynamic::
+commandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os )
+{
+ sotDEBUG(25) << "# In { Cmd " << cmdLine <<endl;
+ std::string filename;
+ if( cmdLine == "debugInertia" )
+ {
+ cmdArgs>>ws; if(cmdArgs.good())
+ {
+ std::string arg; cmdArgs >> arg;
+ if( (arg=="true")||(arg=="1") )
+ { debugInertia = 1; }
+ else if( (arg=="2")||(arg=="grip") )
+ { debugInertia = 2; }
+ else debugInertia=0;
+
+ }
+ else { os << "debugInertia = " << debugInertia << std::endl; }
+ }
+ else if( cmdLine == "setVrmlDir" )
+ { cmdArgs>>filename; setVrmlDirectory( filename ); }
+ else if( cmdLine == "setVrml" )
+ { cmdArgs>>filename; setVrmlMainFile( filename ); }
+ else if( cmdLine == "setXmlSpec" )
+ { cmdArgs>>filename; setXmlSpecificityFile( filename ); }
+ else if( cmdLine == "setXmlRank" )
+ { cmdArgs>>filename; setXmlRankFile( filename ); }
+ else if( cmdLine == "setFiles" )
+ {
+ cmdArgs>>filename; setVrmlDirectory( filename );
+ cmdArgs>>filename; setVrmlMainFile( filename );
+ cmdArgs>>filename; setXmlSpecificityFile( filename );
+ cmdArgs>>filename; setXmlRankFile( filename );
+ }
+ else if( cmdLine == "displayFiles" )
+ {
+ cmdArgs >> ws; bool filespecified = false;
+ if( cmdArgs.good() )
+ {
+ filespecified = true;
+ std::string filetype; cmdArgs >> filetype;
+ sotDEBUG(15) << " Request: " << filetype << std::endl;
+ if( "vrmldir" == filetype ) { os << vrmlDirectory << std::endl; }
+ else if( "xmlspecificity" == filetype ) { os << xmlSpecificityFile << std::endl; }
+ else if( "xmlrank" == filetype ) { os << xmlRankFile << std::endl; }
+ else if( "vrmlmain" == filetype ) { os << vrmlMainFile << std::endl; }
+ else filespecified = false;
+ }
+ if( ! filespecified )
+ {
+ os << " - VRML Directory:\t\t\t" << vrmlDirectory <<endl
+ << " - XML Specificity File:\t\t" << xmlSpecificityFile <<endl
+ << " - XML Rank File:\t\t\t" << xmlRankFile <<endl
+ << " - VRML Main File:\t\t\t" << vrmlMainFile <<endl;
+ }
+ }
+ else if( cmdLine == "parse" )
+ {
+ if(! init )parseConfigFiles(); else cout << " !! Already parsed." <<endl;
+ }
+ else if( cmdLine == "createJacobian" )
+ {
+ std::string Jname; cmdArgs >> Jname;
+ unsigned int rank; cmdArgs >> rank;
+ createJacobianSignal(Jname,rank);
+ }
+ else if( cmdLine == "destroyJacobian" )
+ {
+ std::string Jname; cmdArgs >> Jname;
+ destroyJacobianSignal(Jname);
+ }
+ else if( cmdLine == "createPosition" )
+ {
+ std::string Jname; cmdArgs >> Jname;
+ unsigned int rank; cmdArgs >> rank;
+ createPositionSignal(Jname,rank);
+ }
+ else if( cmdLine == "destroyPosition" )
+ {
+ std::string Jname; cmdArgs >> Jname;
+ destroyPositionSignal(Jname);
+ }
+ else if( cmdLine == "createAcceleration" )
+ {
+ std::string Jname; cmdArgs >> Jname;
+ unsigned int rank; cmdArgs >> rank;
+ createAccelerationSignal(Jname,rank);
+ }
+ else if( cmdLine == "destroyAcceleration" )
+ {
+ std::string Jname; cmdArgs >> Jname;
+ destroyAccelerationSignal(Jname);
+ }
+ else if( cmdLine == "createEndeffJacobian" )
+ {
+ std::string Jname; cmdArgs >> Jname;
+ unsigned int rank; cmdArgs >> rank;
+ createEndeffJacobianSignal(Jname,rank);
+ }
+ else if( cmdLine == "createOpPoint" )
+ {
+ std::string Jname; cmdArgs >> Jname;
+ unsigned int rank; cmdArgs >> rank;
+ createEndeffJacobianSignal(string("J")+Jname,rank);
+ createPositionSignal(Jname,rank);
+ sotDEBUG(15)<<endl;
+ }
+ else if( cmdLine == "destroyOpPoint" )
+ {
+ std::string Jname; cmdArgs >> Jname;
+ destroyJacobianSignal(string("J")+Jname);
+ destroyPositionSignal(Jname);
+ }
+ else if( cmdLine == "ndof" ) { os << m_HDR->numberDof() <<endl; return; }
+ else if( cmdLine == "setComputeCom" )
+ {
+ unsigned int b; cmdArgs >> b;
+ comActivation(b);
+ }
+ else if( cmdLine == "setComputeZmp" )
+ {
+ unsigned int b; cmdArgs >> b;
+ zmpActivation(b);
+ }
+ else if( cmdLine == "setProperty" )
+ {
+ string prop,val; cmdArgs >> prop;
+ if( cmdArgs.good() ) cmdArgs >> val; else val="true";
+ m_HDR->setProperty( prop,val );
+ }
+ else if( cmdLine == "getProperty" )
+ {
+ string prop,val; cmdArgs >> prop;
+ m_HDR->getProperty( prop,val );
+ os<<val<<endl;
+ }
+ else if( cmdLine == "displayProperties" )
+ {
+ std::istringstream iss("ComputeVelocity ComputeCoM ComputeAccelerationCoM ComputeMomentum ComputeZMP ComputeBackwardDynamics");
+ string prop,val; const unsigned int STR_SIZE=30;
+ while( iss.good() )
+ {
+ iss>>prop;
+ m_HDR->getProperty( prop,val );
+ os<<prop;
+ for( unsigned int i=prop.length();i<STR_SIZE;++i ) os<<" ";
+ os<<" -> "<<val <<endl;
+ }
+ }
+ else if( cmdLine == "help" )
+ {
+ os << "Dynamics:"<<endl
+ << " - setVrmlDir - setVrml - setXmlSpec - setXmlRanks <file>" <<endl
+ << "\t\t\t\t:set the config files" <<endl
+ << " - setFiles <%1> ... <%4>\t:set files in the order cited above" <<endl
+ << " - displayFiles\t\t\t:display the 5 config files" <<endl
+ << " - parse\t\t\t:parse the files set unsing the set{Xml|Vrml} commands." <<endl
+ << " - createJacobian <name> <point>:create a signal named <name> " << endl
+ << " - createEndeffJacobian <name> <point>:create a signal named <name> "
+ << "forwarding the jacoian computed at <point>." <<endl
+ << " - destroyJacobian <name>\t:delete the jacobian signal <name>" << endl
+ << " - {create|destroy}Position\t:handle position signals." <<endl
+ << " - {create|destroy}OpPoint\t:handle Operation Point (ie pos+jac) signals." <<endl
+ << " - {create|destroy}Acceleration\t:handle acceleration signals." <<endl
+ << " - {get|set}Property <name> [<val>]: set/get the property." <<endl
+ << " - displayProperties: print the prop-val couples list." <<endl
+ << " - ndof\t\t\t: display the number of DOF of the robot."<< endl;
+
+ Entity::commandLine(cmdLine,cmdArgs,os);
+ }
+ else { Entity::commandLine( cmdLine,cmdArgs,os); }
+
+ sotDEBUGOUT(15);
+
+}
+
+
+
diff --git a/src/force-compensation.cpp b/src/force-compensation.cpp
new file mode 100644
index 0000000..702a5e3
--- /dev/null
+++ b/src/force-compensation.cpp
@@ -0,0 +1,638 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: ForceCompensation.h
+ * Project: SOT
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+#include <sot-dynamic/force-compensation.h>
+#include <sot-core/debug.h>
+#include <dynamic-graph/factory.h>
+#include <sot-core/macros-signal.h>
+
+using namespace sot;
+using namespace dynamicgraph;
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ForceCompensationPlugin,"ForceCompensation");
+
+/* --- PLUGIN --------------------------------------------------------------- */
+/* --- PLUGIN --------------------------------------------------------------- */
+/* --- PLUGIN --------------------------------------------------------------- */
+ForceCompensation::
+ForceCompensation(void)
+ :usingPrecompensation(false)
+{}
+
+
+ForceCompensationPlugin::
+ForceCompensationPlugin( const std::string & name )
+ :Entity(name)
+ ,calibrationStarted(false)
+
+ ,torsorSIN(NULL,"sotForceCompensation("+name+")::input(vector6)::torsorIN")
+ ,worldRhandSIN(NULL,"sotForceCompensation("+name+")::input(MatrixRotation)::worldRhand")
+
+ ,handRsensorSIN(NULL,"sotForceCompensation("+name+")::input(MatrixRotation)::handRsensor")
+ ,translationSensorComSIN(NULL,"sotForceCompensation("+name+")::input(vector3)::sensorCom")
+ ,gravitySIN(NULL,"sotForceCompensation("+name+")::input(vector6)::gravity")
+ ,precompensationSIN(NULL,"sotForceCompensation("+name+")::input(vector6)::precompensation")
+ ,gainSensorSIN(NULL,"sotForceCompensation("+name+")::input(matrix6)::gain")
+ ,deadZoneLimitSIN(NULL,"sotForceCompensation("+name+")::input(vector6)::deadZoneLimit")
+ ,transSensorJointSIN(NULL,"sotForceCompensation("+name+")::input(vector6)::sensorJoint") ,inertiaJointSIN(NULL,"sotForceCompensation("+name+")::input(matrix6)::inertiaJoint")
+ ,velocitySIN(NULL,"sotForceCompensation("+name+")::input(vector6)::velocity")
+ ,accelerationSIN(NULL,"sotForceCompensation("+name+")::input(vector6)::acceleration")
+
+ ,handXworldSOUT( SOT_INIT_SIGNAL_2( ForceCompensation::computeHandXworld,
+ worldRhandSIN,MatrixRotation,
+ translationSensorComSIN,ml::Vector ),
+ "sotForceCompensation("+name+")::output(MatrixForce)::handXworld" )
+ ,handVsensorSOUT( SOT_INIT_SIGNAL_1( ForceCompensation::computeHandVsensor,
+ handRsensorSIN,MatrixRotation),
+ "sotForceCompensation("+name+")::output(MatrixForce)::handVsensor" )
+ ,torsorDeadZoneSIN(NULL,"sotForceCompensation("+name+")::input(vector6)::torsorNullifiedIN")
+
+ ,sensorXhandSOUT( SOT_INIT_SIGNAL_2( ForceCompensation::computeSensorXhand,
+ handRsensorSIN,MatrixRotation,
+ transSensorJointSIN,ml::Vector ),
+ "sotForceCompensation("+name+")::output(MatrixForce)::sensorXhand" )
+// ,inertiaSensorSOUT( SOT_INIT_SIGNAL_2( ForceCompensation::computeInertiaSensor,
+// inertiaJointSIN,ml::Matrix,
+// sensorXhandSOUT,MatrixForce ),
+// "ForceCompensation("+name+")::output(MatrixForce)::inertiaSensor" )
+ ,momentumSOUT( SOT_INIT_SIGNAL_4(ForceCompensation::computeMomentum,
+ velocitySIN,ml::Vector,
+ accelerationSIN,ml::Vector,
+ sensorXhandSOUT,MatrixForce,
+ inertiaJointSIN,ml::Matrix),
+ "sotForceCompensation("+name+")::output(Vector6)::momentum" )
+ ,momentumSIN(NULL,"sotForceCompensation("+name+")::input(vector6)::momentumIN")
+
+ ,torsorCompensatedSOUT( SOT_INIT_SIGNAL_7(ForceCompensation::computeTorsorCompensated,
+ torsorSIN,ml::Vector,
+ precompensationSIN,ml::Vector,
+ gravitySIN,ml::Vector,
+ handXworldSOUT,MatrixForce,
+ handVsensorSOUT,MatrixForce,
+ gainSensorSIN,ml::Matrix,
+ momentumSIN,ml::Vector),
+ "sotForceCompensation("+name+")::output(Vector6)::torsor" )
+ ,torsorDeadZoneSOUT( SOT_INIT_SIGNAL_2(ForceCompensation::computeDeadZone,
+ torsorDeadZoneSIN,ml::Vector,
+ deadZoneLimitSIN,ml::Vector),
+ "sotForceCompensation("+name+")::output(Vector6)::torsorNullified" )
+ ,calibrationTrigerSOUT( boost::bind(&ForceCompensationPlugin::calibrationTriger,
+ this,_1,_2),
+ torsorSIN << worldRhandSIN,
+ "sotForceCompensation("+name+")::output(Dummy)::calibrationTriger")
+{
+ sotDEBUGIN(5);
+
+ signalRegistration( torsorSIN<< worldRhandSIN
+ << handRsensorSIN << translationSensorComSIN
+ << gravitySIN << precompensationSIN << gainSensorSIN
+ << deadZoneLimitSIN
+ << transSensorJointSIN << inertiaJointSIN
+ << velocitySIN << accelerationSIN
+
+ << handXworldSOUT << handVsensorSOUT << torsorDeadZoneSIN
+ << sensorXhandSOUT //<< inertiaSensorSOUT
+ << momentumSOUT << momentumSIN
+ << torsorCompensatedSOUT << torsorDeadZoneSOUT
+ << calibrationTrigerSOUT );
+ torsorDeadZoneSIN.plug(&torsorCompensatedSOUT);
+
+ // By default, I choose: momentum is not compensated.
+ // momentumSIN.plug( &momentumSOUT );
+ ml::Vector v(6); v.fill(0); momentumSIN = v;
+
+ sotDEBUGOUT(5);
+}
+
+
+ForceCompensationPlugin::
+~ForceCompensationPlugin( void )
+{
+ return;
+}
+
+/* --- CALIB --------------------------------------------------------------- */
+/* --- CALIB --------------------------------------------------------------- */
+/* --- CALIB --------------------------------------------------------------- */
+
+MatrixRotation ForceCompensation::I3;
+
+void ForceCompensation::
+clearCalibration( void )
+{
+ torsorList.clear();
+ rotationList.clear();
+}
+
+
+void ForceCompensation::
+addCalibrationValue( const ml::Vector& torsor,
+ const MatrixRotation & worldRhand )
+{
+ sotDEBUGIN(45);
+
+ // sotDEBUG(25) << "Add torsor: t"<<torsorList.size() <<" = " << torsor;
+ // sotDEBUG(25) << "Add Rotation: wRh"<<torsorList.size() <<" = " << worldRhand;
+
+ // torsorList.push_back(torsor);
+ // rotationList.push_back(worldRhand);
+
+ sotDEBUGOUT(45);
+}
+
+ml::Vector ForceCompensation::
+calibrateTransSensorCom( const ml::Vector& gravity,
+ const MatrixRotation& handRsensor )
+{
+ // sotDEBUGIN(25);
+
+ // ml::Vector grav3(3);
+ // ml::Vector Rgrav3(3),tau(3),Rtau(3);
+ // for( unsigned int j=0;j<3;++j ) { grav3(j)=gravity(j); }
+
+ // std::list< ml::Vector >::iterator iterTorsor = torsorList.begin();
+ // std::list< MatrixRotation >::const_iterator iterRotation
+ // = rotationList.begin();
+
+
+ // const unsigned int NVAL = torsorList.size();
+ // if( 0==NVAL )
+ // {
+ // ml::Vector zero(3); zero.fill(0);
+ // return zero;
+ // }
+
+ // if(NVAL!=rotationList.size() )
+ // {
+ // // TODO: ERROR THROW
+ // }
+ // ml::Matrix torsors( 3,NVAL );
+ // ml::Matrix gravitys( 3,NVAL );
+
+ // for( unsigned int i=0;i<NVAL;++i )
+ // {
+ // if( (torsorList.end()==iterTorsor)||(rotationList.end()==iterRotation) )
+ // {
+ // // TODO: ERROR THROW
+ // break;
+ // }
+ // const ml::Vector & torsor = *iterTorsor;
+ // const MatrixRotation & worldRhand = *iterRotation;
+
+ // for( unsigned int j=0;j<3;++j ) { tau(j)=torsor(j+3); }
+ // handRsensor.multiply(tau,Rtau);
+ // worldRhand.transpose().multiply( grav3,Rgrav3 );
+ // for( unsigned int j=0;j<3;++j )
+ // {
+ // torsors( j,i ) = -Rtau(j);
+ // gravitys( j,i ) = Rgrav3(j);
+ // }
+ // sotDEBUG(35) << "R" << i << " = " << worldRhand;
+ // sotDEBUG(35) << "Rtau" << i << " = " << Rtau;
+ // sotDEBUG(35) << "Rg" << i << " = " << Rgrav3;
+
+ // iterTorsor++; iterRotation++;
+ // }
+
+ // sotDEBUG(35) << "Rgs = " << gravitys;
+ // sotDEBUG(35) << "Rtaus = " << torsors;
+
+ // ml::Matrix gravsInv( gravitys.nbCols(),gravitys.nbRows() );
+ // sotDEBUG(25) << "Compute the pinv..." << std::endl;
+ // gravitys.pseudoInverse(gravsInv);
+ // sotDEBUG(25) << "Compute the pinv... [DONE]" << std::endl;
+ // sotDEBUG(25) << "gravsInv = " << gravsInv << std::endl;
+
+ // ml::Matrix Skew(3,3);
+ // torsors.multiply( gravsInv,Skew );
+ // sotDEBUG(25) << "Skew = " << Skew << std::endl;
+
+ // ml::Vector sc(3);
+ // sc(0)=(Skew(2,1)-Skew(1,2))*.5 ;
+ // sc(1)=(Skew(0,2)-Skew(2,0))*.5 ;
+ // sc(2)=(Skew(1,0)-Skew(0,1))*.5 ;
+ // sotDEBUG(15) << "SC = " << sc << std::endl;
+ // /* TAKE the antisym constraint into account inside the minimization pbm. */
+
+ // sotDEBUGOUT(25);
+ // return sc;
+ return gravity;
+}
+
+ml::Vector ForceCompensation::
+calibrateGravity( const MatrixRotation& handRsensor,
+ bool precompensationCalibration,
+ const MatrixRotation& hand0RsensorArg )
+{
+ sotDEBUGIN(25);
+
+ // MatrixRotation hand0Rsensor;
+ // if( &hand0Rsensor==&I3 ) hand0Rsensor.setIdentity();
+ // else hand0Rsensor=hand0RsensorArg;
+
+ // std::list< ml::Vector >::const_iterator iterTorsor = torsorList.begin();
+ // std::list< MatrixRotation >::const_iterator iterRotation
+ // = rotationList.begin();
+
+ // const unsigned int NVAL = torsorList.size();
+ // if(NVAL!=rotationList.size() )
+ // {
+ // // TODO: ERROR THROW
+ // }
+ // if( 0==NVAL )
+ // {
+ // ml::Vector zero(6); zero.fill(0);
+ // return zero;
+ // }
+
+ // ml::Vector force(3),forceHand(3),forceWorld(3);
+ // ml::Vector sumForce(3); sumForce.fill(0);
+
+ // for( unsigned int i=0;i<NVAL;++i )
+ // {
+ // if( (torsorList.end()==iterTorsor)||(rotationList.end()==iterRotation) )
+ // {
+ // // TODO: ERROR THROW
+ // break;
+ // }
+ // const ml::Vector & torsor = *iterTorsor;
+ // const MatrixRotation & R = *iterRotation;
+
+ // /* The sensor read [-] the value, and the grav is [-] the sensor force.
+ // * [-]*[-] = [+] -> force = + torsor(1:3). */
+ // for( unsigned int j=0;j<3;++j ) { force(j)=-torsor(j); }
+ // handRsensor.multiply(force,forceHand);
+ // if( usingPrecompensation )
+ // {
+ // ml::Matrix R_I(3,3); R_I = R.transpose();
+ // R_I -= hand0Rsensor;
+ // R_I.pseudoInverse(.01).multiply( forceHand,forceWorld );
+ // }
+ // else
+ // { R.multiply( forceHand,forceWorld ); }
+
+ // sotDEBUG(35) << "R(" << i << "*3+1:" << i << "*3+3,:) = " << R << "';";
+ // sotDEBUG(35) << "rhFs(" << i << "*3+1:" << i << "*3+3) = " << forceHand;
+ // sotDEBUG(45) << "fworld(" << i << "*3+1:" << i << "*3+3) = " << forceWorld;
+
+ // sumForce+= forceWorld;
+
+ // iterTorsor++; iterRotation++;
+ // }
+
+ // sumForce*= (1./NVAL);
+ // sotDEBUG(35) << "Fmean = " << sumForce;
+ // sumForce.resize(6,false);
+ // sumForce(3)=sumForce(4)=sumForce(5)=0.;
+
+ // sotDEBUG(25)<<"mg = " << sumForce<<std::endl;
+
+ sotDEBUGOUT(25);
+ ml::Vector sumForce(3); sumForce.fill(0);
+ return sumForce;
+}
+
+
+/* --- SIGNALS -------------------------------------------------------------- */
+/* --- SIGNALS -------------------------------------------------------------- */
+/* --- SIGNALS -------------------------------------------------------------- */
+MatrixForce& ForceCompensation::
+computeHandXworld( const MatrixRotation & worldRhand,
+ const ml::Vector & transSensorCom,
+ MatrixForce& res )
+{
+ sotDEBUGIN(35);
+
+ sotDEBUG(25) << "wRrh = " << worldRhand <<std::endl;
+ sotDEBUG(25) << "SC = " << transSensorCom <<std::endl;
+
+ MatrixRotation R; worldRhand.transpose(R);
+ MatrixHomogeneous scRw; scRw.buildFrom( R,transSensorCom);
+ sotDEBUG(25) << "scMw = " << scRw <<std::endl;
+
+ res.buildFrom( scRw );
+ sotDEBUG(15) << "scXw = " << res <<std::endl;
+
+ sotDEBUGOUT(35);
+ return res;
+}
+
+MatrixForce& ForceCompensation::
+computeHandVsensor( const MatrixRotation & handRsensor,
+ MatrixForce& res )
+{
+ sotDEBUGIN(35);
+
+ for( unsigned int i=0;i<3;++i )
+ for( unsigned int j=0;j<3;++j )
+ {
+ res(i,j)=handRsensor(i,j);
+ res(i+3,j+3)=handRsensor(i,j);
+ res(i+3,j)=0.;
+ res(i,j+3)=0.;
+ }
+
+ sotDEBUG(25) << "hVs" << res << std::endl;
+
+ sotDEBUGOUT(35);
+ return res;
+}
+
+MatrixForce& ForceCompensation::
+computeSensorXhand( const MatrixRotation & handRsensor,
+ const ml::Vector & transJointSensor,
+ MatrixForce& res )
+{
+ sotDEBUGIN(35);
+
+ /* Force Matrix to pass from the joint frame (ie frame located
+ * at the position of the motor, in which the acc is computed by Spong)
+ * to the frame SensorHand where all the forces are expressed (ie
+ * frame located at the sensor position bu oriented like the hand). */
+
+ MatrixRotation sensorRhand; sensorRhand.setIdentity();
+ //handRsensor.transpose(sensorRhand);
+ MatrixHomogeneous sensorMhand;
+ sensorMhand.buildFrom( sensorRhand,transJointSensor );
+
+ res.buildFrom( sensorMhand );
+ sotDEBUG(25) << "shXJ" << res << std::endl;
+
+ sotDEBUGOUT(35);
+ return res;
+}
+
+// ml::Matrix& ForceCompensation::
+// computeInertiaSensor( const ml::Matrix& inertiaJoint,
+// const MatrixForce& sensorXhand,
+// ml::Matrix& res )
+// {
+// sotDEBUGIN(35);
+
+// /* Inertia felt at the sensor position, expressed in the orientation
+// * of the hand. */
+
+// res.resize(6,6);
+// sensorXhand.multiply( inertiaJoint,res );
+
+// sotDEBUGOUT(35);
+// return res;
+// }
+
+
+ml::Vector& ForceCompensation::
+computeTorsorCompensated( const ml::Vector& torqueInput,
+ const ml::Vector& torquePrecompensation,
+ const ml::Vector& gravity,
+ const MatrixForce& handXworld,
+ const MatrixForce& handVsensor,
+ const ml::Matrix& gainSensor,
+ const ml::Vector& momentum,
+ ml::Vector& res )
+
+{
+ sotDEBUGIN(35);
+ /* Torsor in the sensor frame: K*(-torsred-gamma)+sVh*hXw*mg */
+ /* Torsor in the hand frame: hVs*K*(-torsred-gamma)+hXw*mg */
+ /* With gamma expressed in the sensor frame (gamma_s = sVh*gamma_h) */
+
+ sotDEBUG(25) << "t_nc = " << torqueInput;
+ ml::Vector torquePrecompensated(6);
+ //if( usingPrecompensation )
+ { torqueInput.addition( torquePrecompensation,torquePrecompensated ); }
+ //else { torquePrecompensated = torqueInput; }
+ sotDEBUG(25) << "t_pre = " << torquePrecompensated;
+
+ ml::Vector torqueS(6), torqueRH(6);
+ gainSensor.multiply( torquePrecompensated,torqueS );
+ handVsensor.multiply( torqueS,res );
+ sotDEBUG(25) << "t_rh = " << res;
+
+ ml::Vector grh(6);
+ handXworld.multiply(gravity,grh);
+ grh *= -1;
+ sotDEBUG(25) << "g_rh = " << grh;
+
+ res+=grh;
+ sotDEBUG(25) << "fcomp = " << res;
+
+ res+=momentum;
+ sotDEBUG(25) << "facc = " << res;
+
+
+ /* TODO res += m xddot */
+
+ sotDEBUGOUT(35);
+ return res;
+}
+
+ml::Vector& ForceCompensation::
+crossProduct_V_F( const ml::Vector& velocity,
+ const ml::Vector& force,
+ ml::Vector& res )
+{
+ /* [ v;w] x [ f;tau ] = [ w x f; v x f + w x tau ] */
+ ml::Vector v(3),w(3),f(3),tau(3);
+ for( unsigned int i=0;i<3;++i )
+ {
+ v(i)=velocity(i); w(i) = velocity(i+3);
+ f(i) = force(i); tau(i) = force(i+3);
+ }
+ ml::Vector res1(3),res2a(3),res2b;
+ w.crossProduct( f,res1 );
+ v.crossProduct( f,res2a );
+ w.crossProduct( tau,res2b );
+ res2a+= res2b;
+
+ res.resize(6);
+ for( unsigned int i=0;i<3;++i )
+ {
+ res(i)=res1(i); res(i+3)=res2a(i);
+ }
+ return res;
+}
+
+
+ml::Vector& ForceCompensation::
+computeMomentum( const ml::Vector& velocity,
+ const ml::Vector& acceleration,
+ const MatrixForce& sensorXhand,
+ const ml::Matrix& inertiaJoint,
+ ml::Vector& res )
+{
+ sotDEBUGIN(35);
+
+ /* Fs + Fext = I acc + V x Iv */
+ ml::Vector Iacc(6); inertiaJoint.multiply( acceleration,Iacc );
+ res.resize(6); sensorXhand.multiply( Iacc,res );
+
+ ml::Vector Iv(6); inertiaJoint.multiply( velocity,Iv);
+ ml::Vector vIv(6); crossProduct_V_F( velocity,Iv,vIv );
+ ml::Vector XvIv(6); sensorXhand.multiply( vIv,XvIv);
+ res+= XvIv;
+
+ sotDEBUGOUT(35);
+ return res;
+}
+
+ml::Vector& ForceCompensation::
+computeDeadZone( const ml::Vector& torqueInput,
+ const ml::Vector& deadZone,
+ ml::Vector& res )
+
+{
+ sotDEBUGIN(35);
+
+ if( torqueInput.size()>deadZone.size() ) return res;
+ res.resize(torqueInput.size());
+ for( unsigned int i=0;i<torqueInput.size();++i )
+ {
+ const double th = fabsf(deadZone(i));
+ if( (torqueInput(i)<th)&&(torqueInput(i)>-th) )
+ { res(i)=0; }
+ else if(torqueInput(i)<0) res(i)=torqueInput(i)+th;
+ else res(i)=torqueInput(i)-th;
+ }
+
+ sotDEBUGOUT(35);
+ return res;
+}
+
+
+ForceCompensationPlugin::sotDummyType& ForceCompensationPlugin::
+calibrationTriger( ForceCompensationPlugin::sotDummyType& dummy,int time )
+{
+ // sotDEBUGIN(45);
+ // if(! calibrationStarted ) { sotDEBUGOUT(45); return dummy=0; }
+
+ // addCalibrationValue( torsorSIN(time),worldRhandSIN(time) );
+ // sotDEBUGOUT(45);
+ return dummy=1;
+}
+
+/* --- COMMANDLINE ---------------------------------------------------------- */
+/* --- COMMANDLINE ---------------------------------------------------------- */
+/* --- COMMANDLINE ---------------------------------------------------------- */
+
+void ForceCompensationPlugin::
+commandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os )
+{
+ if( "help"==cmdLine )
+ {
+ os << "ForceCompensation: "
+ << " - clearCalibration" << std::endl
+ << " - {start|stop}Calibration [wait <time_sec>]" << std::endl
+ << " - calibrateGravity\t[only {x|y|z}]" << std::endl
+ << " - calibratePosition" << std::endl
+ << " - precomp [{true|false}]: get/set the "
+ << "precompensation due to sensor calib." << std::endl;
+ }
+ // else if( "clearCalibration" == cmdLine )
+ // {
+ // clearCalibration();
+ // }
+ // else if( "startCalibration" == cmdLine )
+ // {
+ // calibrationStarted = true;
+ // cmdArgs >> std::ws;
+ // if( cmdArgs.good() )
+ // {
+ // std::string cmdWait; cmdArgs>>cmdWait>>std::ws;
+ // if( (cmdWait == "wait")&&(cmdArgs.good()) )
+ // {
+ // double timeSec; cmdArgs >> timeSec;
+ // unsigned int timeMSec= (unsigned int)(round(timeSec*1000*1000));
+ // sotDEBUG(15) << "Calibration: wait for " << timeMSec << "us."<<std::endl;
+ // usleep( timeMSec );
+ // calibrationStarted = false;
+ // }
+ // }
+ // }
+ // else if( "stopCalibration" == cmdLine )
+ // {
+ // calibrationStarted = false;
+ // }
+ // else if( "calibrateGravity" == cmdLine )
+ // {
+ // if( calibrationStarted )
+ // {
+ // os<< "Calibration phase is on, stop it first."<<std::endl;
+ // return;
+ // }
+ // ml::Vector grav = calibrateGravity( handRsensorSIN.accessCopy(),
+ // usingPrecompensation );
+
+ // cmdArgs >> std::ws;
+ // if( cmdArgs.good() )
+ // {
+ // std::string cmdOnly; cmdArgs>>cmdOnly>>std::ws;
+ // if( (cmdOnly == "only")&&(cmdArgs.good()) )
+ // {
+ // std::string xyz; cmdArgs >> xyz;
+ // if( "x"==xyz ) { grav(1)=grav(2)=0.; }
+ // else if( "y"==xyz ) { grav(0)=grav(2)=0.; }
+ // else if( "z"==xyz ) { grav(0)=grav(1)=0.; }
+ // }
+ // }
+
+ // gravitySIN = grav;
+ // }
+ // else if( "calibratePosition" == cmdLine )
+ // {
+ // if( calibrationStarted )
+ // {
+ // return;
+ // os<< "Calibration phase is on, stop it first."<<std::endl;
+ // }
+ // ml::Vector position(3);
+ // position = calibrateTransSensorCom( gravitySIN.accessCopy(),
+ // handRsensorSIN.accessCopy() );
+ // transSensorComSIN = position;
+ // }
+ else if( "precomp" == cmdLine )
+ {
+ cmdArgs>>std::ws;
+ if( cmdArgs.good() )
+ { cmdArgs >> usingPrecompensation; }
+ else { os << "precompensation = " << usingPrecompensation <<std::endl; }
+ }
+ else if( "compensateMomentum" == cmdLine )
+ {
+ cmdArgs>>std::ws;
+ if( cmdArgs.good() )
+ {
+ bool use; cmdArgs >> use;
+ if( use ) momentumSIN.plug( &momentumSOUT );
+ else
+ {
+ ml::Vector m(6); m.resize(0); momentumSIN = m;
+ }
+ }
+ else
+ {
+ os << "compensateMomentum = " << (momentumSIN.getPtr()!=&momentumSIN)
+ <<std::endl;
+ }
+ }
+ else{ Entity::commandLine( cmdLine,cmdArgs,os ); }
+
+
+}
+
diff --git a/src/integrator-force-exact.cpp b/src/integrator-force-exact.cpp
new file mode 100644
index 0000000..fbe5140
--- /dev/null
+++ b/src/integrator-force-exact.cpp
@@ -0,0 +1,251 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: IntegratorForceExact.h
+ * Project: SOT
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+#include <sot-dynamic/integrator-force-exact.h>
+#include <sot-core/debug.h>
+#include <dynamic-graph/factory.h>
+#include <sot-core/exception-dynamic.h>
+
+using namespace sot;
+using namespace dynamicgraph;
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(IntegratorForceExact,"IntegratorForceExact");
+
+IntegratorForceExact::
+IntegratorForceExact( const std::string & name )
+ :IntegratorForce(name)
+{
+ sotDEBUGIN(5);
+
+ velocitySOUT.
+ setFunction( boost::bind(&IntegratorForceExact::computeVelocityExact,
+ this,_1,_2));
+ velocitySOUT.removeDependency( velocityDerivativeSOUT );
+
+ sotDEBUGOUT(5);
+}
+
+
+IntegratorForceExact::
+~IntegratorForceExact( void )
+{
+ sotDEBUGIN(5);
+
+ sotDEBUGOUT(5);
+ return;
+}
+
+/* --- EIGEN VALUE ---------------------------------------------------------- */
+/* --- EIGEN VALUE ---------------------------------------------------------- */
+/* --- EIGEN VALUE ---------------------------------------------------------- */
+namespace ublas = boost::numeric::ublas;
+
+
+extern "C"
+{
+ void dgeev_( const char* jobvl, const char* jobvr, const int* n, double* a,
+ const int* lda, double* wr, double* wi, double* vl, const int* ldvl,
+ double* vr, const int* ldvr, double* work, const int* lwork, int* info );
+}
+
+
+int geev(::boost::numeric::ublas::matrix<double, ::boost::numeric::ublas::column_major> &a,
+ ublas::vector< std::complex<double> > &w,
+ ::boost::numeric::ublas::matrix<double,::boost::numeric::ublas::column_major> &vl2,
+ ::boost::numeric::ublas::matrix<double,::boost::numeric::ublas::column_major> &vr2
+ )
+{
+ char jobvl='V';
+ char jobvr='V';
+ int const n = a.size1();
+
+ ::boost::numeric::ublas::vector<double> wr(n);
+ ::boost::numeric::ublas::vector<double> wi(n);
+ double* vl_real = MRAWDATA(vl2);
+ const int ldvl =vl2.size1();
+ double* vr_real = MRAWDATA(vr2);
+ const int ldvr = vr2.size1();
+
+ // workspace query
+ int lwork = -1;
+ double work_temp;
+ int result=0;
+ dgeev_(&jobvl, &jobvr, &n,
+ MRAWDATA(a), &n,
+ VRAWDATA(wr), VRAWDATA(wi),
+ vl_real, &ldvl, vr_real, &ldvr,
+ &work_temp, &lwork, &result);
+ if (result != 0)
+ return result;
+
+ lwork = (int) work_temp;
+ ::boost::numeric::ublas::vector<double> work(lwork);
+ dgeev_(&jobvl, &jobvr, &n,
+ MRAWDATA(a), &n,
+ VRAWDATA(wr), VRAWDATA(wi),
+ vl_real, &ldvl, vr_real, &ldvr,
+ VRAWDATA(work), &lwork,
+ &result);
+
+ for (int i = 0; i < n; i++)
+ w[i] = std::complex<double>(wr[i], wi[i]);
+
+ return result;
+}
+
+static void eigenDecomp( const ml::Matrix& M,
+ ml::Matrix& P,
+ ml::Vector& eig )
+{
+ unsigned int SIZE = M.nbCols();
+ ublas::matrix<double, ublas::column_major> Y=M.matrix;
+ ublas::vector< std::complex<double> > evals(SIZE);
+ ublas::matrix<double, ublas::column_major> vl(SIZE,SIZE);
+ ublas::matrix<double, ublas::column_major> vr(SIZE,SIZE);
+
+ // const int errCode = lapack::geev(Y, evals, &vl, &vr, lapack::optimal_workspace());
+ const int errCode = geev(Y,evals,vl,vr);
+ if( errCode<0 )
+ { SOT_THROW ExceptionDynamic( ExceptionDynamic::INTEGRATION,
+ "Invalid argument to geev","" ); }
+ else if( errCode>0 )
+ { SOT_THROW ExceptionDynamic( ExceptionDynamic::INTEGRATION,
+ "No convergence for given matrix",""); }
+
+ P.resize(SIZE,SIZE); eig.resize(SIZE);
+ for( unsigned int i=0;i<SIZE;++i )
+ {
+ for( unsigned int j=0;j<SIZE;++j ){ P(i,j)=vr(i,j); }
+ eig(i)=evals(i).real();
+ if( fabsf(evals(i).imag())>1e-5 )
+ {
+ SOT_THROW ExceptionDynamic( ExceptionDynamic::INTEGRATION,
+ "Error imaginary part not null. ",
+ "(at position %d: %f)",i,evals(i).imag() );
+ }
+ }
+ return ;
+}
+
+static void expMatrix( const ml::Matrix& MiB,
+ ml::Matrix& Mexp )
+{
+ unsigned int SIZE = MiB.nbCols();
+
+ ml::Matrix Pmib(MiB.nbCols(),MiB.nbCols());
+ ml::Vector eig_mib(MiB.nbCols());
+ eigenDecomp( MiB,Pmib,eig_mib );
+ sotDEBUG(45) << "V = " << Pmib;
+ sotDEBUG(45) << "d = " << eig_mib;
+
+ ml::Matrix Pinv(SIZE,SIZE); Pmib.inverse(Pinv);
+ sotDEBUG(45) << "Vinv = " << Pinv;
+
+ Mexp.resize(SIZE,SIZE);
+ for( unsigned int i=0;i<SIZE;++i )
+ for( unsigned int j=0;j<SIZE;++j )
+ Pmib(i,j)*= exp( eig_mib(j) );
+ Pmib.multiply(Pinv,Mexp);
+
+ sotDEBUG(45) << "expMiB = " << Mexp;
+ return ;
+}
+
+/* --- SIGNALS -------------------------------------------------------------- */
+/* --- SIGNALS -------------------------------------------------------------- */
+/* --- SIGNALS -------------------------------------------------------------- */
+
+/* The derivative of the signal is such that: M v_dot + B v = f. We deduce:
+ * v_dot = M^-1 (f - Bv)
+ * Using Exact method:
+ * dv = exp( M^-1.B.t) ( v0-B^-1.f ) + B^-1.f
+ */
+
+ml::Vector& IntegratorForceExact::
+computeVelocityExact( ml::Vector& res,
+ const int& time )
+{
+ sotDEBUGIN(15);
+
+ const ml::Vector & force = forceSIN( time );
+ const ml::Matrix & massInverse = massInverseSIN( time );
+ const ml::Matrix & friction = frictionSIN( time );
+ unsigned int nf = force.size(), nv = friction.nbCols();
+ res.resize(nv); res.fill(0);
+
+ if(! velocityPrecSIN )
+ {
+ ml::Vector zero( nv ); zero.fill(0);
+ velocityPrecSIN = zero;
+ }
+ const ml::Vector & vel = velocityPrecSIN( time );
+ double & dt = this->IntegratorForce::timeStep; // this is &
+
+ sotDEBUG(15) << "force = " << force;
+ sotDEBUG(15) << "vel = " << vel;
+ sotDEBUG(25) << "Mi = " << massInverse;
+ sotDEBUG(25) << "B = " << friction;
+ sotDEBUG(25) << "dt = " << dt << std::endl;
+
+ ml::Matrix MiB( nv,nv );
+ massInverse.multiply(friction,MiB);
+ sotDEBUG(25) << "MiB = " << MiB;
+
+ ml::Matrix MiBexp( nv,nv );
+ MiB*=-dt; expMatrix(MiB,MiBexp);
+ sotDEBUG(25) << "expMiB = " << MiBexp;
+
+ ml::Matrix Binv( nv,nv ); friction.inverse(Binv);
+ ml::Vector Bif( nf ); Binv.multiply( force,Bif );
+ sotDEBUG(25) << "Binv = " << Binv;
+ sotDEBUG(25) << "Bif = " << Bif;
+
+ ml::Vector v0_bif = vel;
+ v0_bif -= Bif;
+ sotDEBUG(25) << "Kst = " << v0_bif;
+
+ res.resize( MiBexp.nbRows() );
+ MiBexp.multiply( v0_bif,res );
+
+ res += Bif;
+ velocityPrecSIN = res ;
+ sotDEBUG(25) << "vfin = " << res;
+
+
+ sotDEBUGOUT(15);
+ return res;
+}
+
+
+/* --- PARAMS --------------------------------------------------------------- */
+/* --- PARAMS --------------------------------------------------------------- */
+/* --- PARAMS --------------------------------------------------------------- */
+// void IntegratorForceExact::
+// commandLine( const std::string& cmdLine,
+// std::istringstream& cmdArgs,
+// std::ostream& os )
+// {
+// sotDEBUG(25) << "Cmd " << cmdLine <<std::endl;
+
+// if( cmdLine == "help" )
+// {
+// os << "IntegratorForceExact: " << std::endl;
+// }
+// else { IntegratorForce::commandLine( cmdLine,cmdArgs,os); }
+// }
+
diff --git a/src/integrator-force-rk4.cpp b/src/integrator-force-rk4.cpp
new file mode 100644
index 0000000..005c275
--- /dev/null
+++ b/src/integrator-force-rk4.cpp
@@ -0,0 +1,136 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: IntegratorForceRK4.h
+ * Project: SOT
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+#include <sot-dynamic/integrator-force-rk4.h>
+#include <sot-core/debug.h>
+#include <dynamic-graph/factory.h>
+
+
+using namespace sot;
+using namespace dynamicgraph;
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(IntegratorForceRK4,"IntegratorForceRK4");
+
+IntegratorForceRK4::
+IntegratorForceRK4( const std::string & name )
+ :IntegratorForce(name)
+{
+ sotDEBUGIN(5);
+
+ velocityDerivativeSOUT.
+ setFunction( boost::bind(&IntegratorForceRK4::computeDerivativeRK4,
+ this,_1,_2));
+
+ sotDEBUGOUT(5);
+}
+
+
+IntegratorForceRK4::
+~IntegratorForceRK4( void )
+{
+ sotDEBUGIN(5);
+
+ sotDEBUGOUT(5);
+ return;
+}
+
+/* --- SIGNALS -------------------------------------------------------------- */
+/* --- SIGNALS -------------------------------------------------------------- */
+/* --- SIGNALS -------------------------------------------------------------- */
+
+/* The derivative of the signal is such that: M v_dot + B v = f. We deduce:
+ * v_dot = M^-1 (f - Bv)
+ * Using RK4 method (doc: wikipedia ;) ): dv= dt/6 ( k1 + 2.k2 + 2.k3 + k4)
+ */
+static const double rk_fact[4] = { 1.,2.,2.,1. };
+
+ml::Vector& IntegratorForceRK4::
+computeDerivativeRK4( ml::Vector& res,
+ const int& time )
+{
+ sotDEBUGIN(15);
+
+ const ml::Vector & force = forceSIN( time );
+ const ml::Matrix & massInverse = massInverseSIN( time );
+ const ml::Matrix & friction = frictionSIN( time );
+ unsigned int nf = force.size(), nv = friction.nbCols();
+ res.resize(nv); res.fill(0);
+
+ if(! velocityPrecSIN )
+ {
+ ml::Vector zero( nv ); zero.fill(0);
+ velocityPrecSIN = zero;
+ }
+ const ml::Vector & vel = velocityPrecSIN( time );
+ double & dt = this->IntegratorForce::timeStep; // this is &
+
+ sotDEBUG(15) << "force = " << force;
+ sotDEBUG(15) << "vel = " << vel;
+ sotDEBUG(25) << "Mi = " << massInverse;
+ sotDEBUG(25) << "B = " << friction;
+ sotDEBUG(25) << "dt = " << dt << std::endl;
+
+ std::vector<ml::Vector> v(4);
+ ml::Vector ki( nv ), fi( nf );
+ double sumFact = 0;
+ v[0]=vel;
+
+ for( unsigned int i=0;i<4;++i )
+ {
+ sotDEBUG(35) << "v"<<i<<" = " << v[i];
+ friction.multiply( v[i],fi ); fi*=-1;
+ fi += force;
+ sotDEBUG(35) << "f"<<i<<" = " << fi;
+ massInverse.multiply( fi,ki );
+ sotDEBUG(35) << "k"<<i<<" = " << ki;
+ if( i+1<4 )
+ {
+ v[i+1] = ki; v[i+1] *= (dt/rk_fact[i+1]);
+ v[i+1] += vel;
+ }
+ ki *= rk_fact[i];
+ res += ki;
+ sotDEBUG(35) << "sum_k"<<i<<" = " << res;
+ sumFact += rk_fact[i];
+ }
+
+ sotDEBUG(35) << "sum_ki = " << res;
+ res *= (1/sumFact);
+
+ sotDEBUGOUT(15);
+ return res;
+}
+
+
+/* --- PARAMS --------------------------------------------------------------- */
+/* --- PARAMS --------------------------------------------------------------- */
+/* --- PARAMS --------------------------------------------------------------- */
+// void IntegratorForceRK4::
+// commandLine( const std::string& cmdLine,
+// std::istringstream& cmdArgs,
+// std::ostream& os )
+// {
+// sotDEBUG(25) << "Cmd " << cmdLine <<std::endl;
+
+// if( cmdLine == "help" )
+// {
+// os << "IntegratorForceRK4: " << std::endl;
+// }
+// else { IntegratorForce::commandLine( cmdLine,cmdArgs,os); }
+// }
+
diff --git a/src/integrator-force.cpp b/src/integrator-force.cpp
new file mode 100644
index 0000000..72fde2b
--- /dev/null
+++ b/src/integrator-force.cpp
@@ -0,0 +1,171 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: IntegratorForce.h
+ * Project: SOT
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+#include <sot-dynamic/integrator-force.h>
+#include <sot-core/debug.h>
+#include <dynamic-graph/factory.h>
+
+using namespace sot;
+using namespace dynamicgraph;
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(IntegratorForce,"IntegratorForce");
+
+const double IntegratorForce:: TIME_STEP_DEFAULT = 5e-3;
+
+IntegratorForce::
+IntegratorForce( const std::string & name )
+ :Entity(name)
+ ,timeStep( TIME_STEP_DEFAULT )
+ ,forceSIN(NULL,"sotIntegratorForce("+name+")::input(vector)::force")
+ ,massInverseSIN(NULL,"sotIntegratorForce("+name+")::input(matrix)::massInverse")
+ ,frictionSIN(NULL,"sotIntegratorForce("+name+")::input(matrix)::friction")
+ ,velocityPrecSIN(NULL,"sotIntegratorForce("+name+")::input(matrix)::vprec")
+
+ ,velocityDerivativeSOUT
+ ( boost::bind(&IntegratorForce::computeDerivative,this,_1,_2),
+ velocityPrecSIN<<forceSIN<<massInverseSIN<<frictionSIN,
+ "sotIntegratorForce("+name+")::output(Vector)::velocityDerivative" )
+ ,velocitySOUT( boost::bind(&IntegratorForce::computeIntegral,this,_1,_2),
+ velocityPrecSIN<<velocityDerivativeSOUT,
+ "sotIntegratorForce("+name+")::output(Vector)::velocity" )
+
+ ,massSIN(NULL,"sotIntegratorForce("+name+")::input(matrix)::mass")
+ ,massInverseSOUT( boost::bind(&IntegratorForce::computeMassInverse,this,_1,_2),
+ massSIN,
+ "sotIntegratorForce("+name+")::input(matrix)::massInverseOUT")
+{
+ sotDEBUGIN(5);
+
+ signalRegistration( forceSIN << massInverseSIN
+ << frictionSIN << velocityPrecSIN
+ << velocityDerivativeSOUT << velocitySOUT
+ << massInverseSOUT << massSIN );
+
+ massInverseSIN.plug( &massInverseSOUT );
+
+ sotDEBUGOUT(5);
+}
+
+
+IntegratorForce::
+~IntegratorForce( void )
+{
+ sotDEBUGIN(5);
+
+ sotDEBUGOUT(5);
+ return;
+}
+
+/* --- SIGNALS -------------------------------------------------------------- */
+/* --- SIGNALS -------------------------------------------------------------- */
+/* --- SIGNALS -------------------------------------------------------------- */
+
+/* The derivative of the signal is such that: M v_dot + B v = f. We deduce:
+ * v_dot = M^-1 (f - Bv)
+ */
+ml::Vector& IntegratorForce::
+computeDerivative( ml::Vector& res,
+ const int& time )
+{
+ sotDEBUGIN(15);
+
+ const ml::Vector & force = forceSIN( time );
+ const ml::Matrix & massInverse = massInverseSIN( time );
+ const ml::Matrix & friction = frictionSIN( time );
+
+ sotDEBUG(15) << "force = " << force << std::endl;
+
+ ml::Vector f_bv( force.size() );
+
+ if( velocityPrecSIN )
+ {
+ const ml::Vector & vel = velocityPrecSIN( time );
+ sotDEBUG(15) << "vel = " << vel << std::endl;
+ friction .multiply( vel,f_bv ); f_bv *= -1;
+ } else { f_bv.fill(0) ; } // vel is not set yet.
+
+ f_bv+=force;
+ massInverse.multiply( f_bv, res );
+
+ sotDEBUGOUT(15);
+ return res;
+}
+
+ml::Vector& IntegratorForce::
+computeIntegral( ml::Vector& res,
+ const int& time )
+{
+ sotDEBUGIN(15);
+
+ const ml::Vector & dvel =velocityDerivativeSOUT( time );
+ res = dvel;
+ res *= timeStep;
+ if( velocityPrecSIN )
+ {
+ const ml::Vector & vel = velocityPrecSIN( time );
+ res += vel;
+ }
+ else { /* vprec not set yet. */ }
+ velocityPrecSIN = res ;
+
+ sotDEBUGOUT(15);
+ return res;
+}
+
+ml::Matrix& IntegratorForce::
+computeMassInverse( ml::Matrix& res,
+ const int& time )
+{
+ sotDEBUGIN(15);
+
+ const ml::Matrix & mass = massSIN( time );
+ mass.inverse( res );
+
+ sotDEBUGOUT(15);
+ return res;
+}
+
+
+/* --- PARAMS --------------------------------------------------------------- */
+/* --- PARAMS --------------------------------------------------------------- */
+/* --- PARAMS --------------------------------------------------------------- */
+void IntegratorForce::
+commandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os )
+{
+ sotDEBUG(25) << "Cmd " << cmdLine <<std::endl;
+
+ if( cmdLine == "help" )
+ {
+ os << "IntegratorForce: "
+ << " - dt [<time>]: get/set the timestep value. " << std::endl;
+ }
+ else if( cmdLine == "dt" )
+ {
+ cmdArgs >>std::ws;
+ if( cmdArgs.good() )
+ {
+ double val; cmdArgs>>val;
+ if( val >0. ) timeStep = val;
+ }
+ else { os << "TimeStep = " << timeStep << std::endl;}
+ }
+ else { Entity::commandLine( cmdLine,cmdArgs,os); }
+}
+
diff --git a/src/mass-apparent.cpp b/src/mass-apparent.cpp
new file mode 100644
index 0000000..39bae9e
--- /dev/null
+++ b/src/mass-apparent.cpp
@@ -0,0 +1,108 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: MassApparent.h
+ * Project: SOT
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+#include <sot-dynamic/mass-apparent.h>
+#include <sot-core/debug.h>
+#include <dynamic-graph/factory.h>
+
+using namespace sot;
+using namespace dynamicgraph;
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(MassApparent,"MassApparent");
+
+
+MassApparent::
+MassApparent( const std::string & name )
+ :Entity(name)
+ ,jacobianSIN(NULL,"sotMassApparent("+name+")::input(vector)::jacobian")
+ ,inertiaInverseSIN(NULL,"sotMassApparent("+name+")::input(vector)::inertiaInverse")
+ ,massInverseSOUT( boost::bind(&MassApparent::computeMassInverse,this,_1,_2),
+ jacobianSIN<<inertiaInverseSIN,
+ "sotMassApparent("+name+")::output(Vector)::massInverse" )
+ ,massSOUT( boost::bind(&MassApparent::computeMass,this,_1,_2),
+ massInverseSOUT,
+ "sotMassApparent("+name+")::output(Vector)::mass" )
+
+ ,inertiaSIN(NULL,"sotMassApparent("+name+")::input(vector)::inertia")
+ ,inertiaInverseSOUT( boost::bind(&MassApparent::computeInertiaInverse,this,_1,_2),
+ inertiaSIN,
+ "sotMassApparent("+name+")::input(vector)::inertiaInverseOUT")
+{
+ sotDEBUGIN(5);
+
+ signalRegistration( jacobianSIN << inertiaInverseSIN << massInverseSOUT << massSOUT
+ << inertiaSIN << inertiaInverseSOUT);
+ inertiaInverseSIN.plug( &inertiaInverseSOUT );
+
+ sotDEBUGOUT(5);
+}
+
+
+MassApparent::
+~MassApparent( void )
+{
+ return;
+}
+
+/* --- SIGNALS -------------------------------------------------------------- */
+/* --- SIGNALS -------------------------------------------------------------- */
+/* --- SIGNALS -------------------------------------------------------------- */
+ml::Matrix& MassApparent::
+computeMassInverse( ml::Matrix& res,
+ const int& time )
+{
+ sotDEBUGIN(15);
+
+ const ml::Matrix & J = jacobianSIN( time );
+ const ml::Matrix & A = inertiaInverseSIN( time );
+
+ ml::Matrix AJt( J.nbCols(),J.nbRows() );
+ A.multiply( J.transpose(),AJt );
+
+ res.resize( J.nbRows(),J.nbRows() );
+ J.multiply( AJt,res );
+
+ sotDEBUGOUT(15);
+ return res;
+}
+
+ml::Matrix& MassApparent::
+computeMass( ml::Matrix& res,
+ const int& time )
+{
+ sotDEBUGIN(15);
+
+ const ml::Matrix & omega = massInverseSOUT( time );
+ omega.inverse( res );
+
+ sotDEBUGOUT(15);
+ return res;
+}
+
+ml::Matrix& MassApparent::
+computeInertiaInverse( ml::Matrix& res,
+ const int& time )
+{
+ sotDEBUGIN(15);
+
+ const ml::Matrix & A = inertiaSIN( time );
+ A.inverse( res );
+
+ sotDEBUGOUT(15);
+ return res;
+}
diff --git a/src/matrix-inertia.cpp b/src/matrix-inertia.cpp
new file mode 100644
index 0000000..5d88b63
--- /dev/null
+++ b/src/matrix-inertia.cpp
@@ -0,0 +1,305 @@
+#include <fstream>
+#include <vector>
+#include <map>
+
+#include <sot-dynamic/matrix-inertia.h>
+#include <dynamicsJRLJapan/Joint.h>
+#include <dynamicsJRLJapan/HumanoidDynamicMultiBody.h>
+#include <robotDynamics/jrlRobotDynamicsObjectConstructor.h>
+
+#include <sot-core/debug.h>
+
+using namespace dynamicsJRLJapan;
+using namespace sot;
+using namespace dynamicgraph;
+
+static matrix3d skewSymmetric(const vector3d& v)
+{
+ matrix3d res;
+
+ res(0, 0) = 0.0;
+ res(1, 1) = 0.0;
+ res(2, 2) = 0.0;
+
+ res(0, 1) = -v[2];
+ res(0, 2) = v[1];
+
+ res(1, 0) = v[2];
+ res(1, 2) = -v[0];
+
+ res(2, 0) = -v[1];
+ res(2, 1) = v[0];
+
+ return res;
+}
+
+/* -------------------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+
+
+MatrixInertia::
+MatrixInertia( CjrlHumanoidDynamicRobot* aHDR )
+ :aHDR_( aHDR )
+ ,aHDMB_( 0x0 )
+{
+ sotDEBUGIN(25);
+ if( aHDR!=NULL ) init( aHDR );
+ sotDEBUGOUT(25);
+}
+
+void MatrixInertia::
+init( CjrlHumanoidDynamicRobot* aHDR )
+{
+ sotDEBUGIN(25);
+ aHDR_ = aHDR;
+ aHDMB_ = dynamic_cast<dynamicsJRLJapan::HumanoidDynamicMultiBody*>(aHDR_);
+
+ /* STEP 2: get the joints and resize internal vectors according to
+ * number of joints. */
+ joints_ = aHDMB_->jointVector();
+ sotDEBUG(25) << "Joints:" << joints_.size() << endl;
+
+ parentIndex_.resize(joints_.size());
+ inertia_.resize(joints_.size() + 5, joints_.size() + 5);
+ phi.resize( joints_.size() );
+ iVpi.resize( joints_.size() );
+ iVpiT.resize( joints_.size() );
+ Ic.resize( joints_.size() );
+
+ /* STEP 3: create the index of parents. */
+ initParents();
+
+ /* STEP 4: initialize phi (dof table) for each joint. */
+ initDofTable();
+ sotDEBUGOUT(25);
+}
+
+void MatrixInertia::
+initParents( void )
+{
+ sotDEBUGIN(25);
+ std::map<CjrlJoint*, int> m;
+ for(size_t i = 0; i < joints_.size(); ++i)
+ m[joints_[i]] = i;
+
+ sotDEBUG(15) << "Parent Indexes:" << std::endl;
+ for(size_t i = 0; i < joints_.size(); ++i)
+ {
+ if(joints_[i]->parentJoint() == 0x0)
+ {
+ parentIndex_[i] = -1;
+ sotDEBUG(15) << "parent of\t" << i << "\t("
+ << static_cast<Joint*>(joints_[i])->getName()
+ << "):\t" << -1 << std::endl;
+ }
+ else
+ {
+ parentIndex_[i] = m[joints_[i]->parentJoint()];
+ sotDEBUG(15) << "parent of\t" << i << ":\t("
+ << static_cast<Joint*>(joints_[i])->getName()
+ << "):\t" << m[joints_[i]->parentJoint()]
+ << "\t(" << static_cast<Joint*>
+ (joints_[m[joints_[i]->parentJoint()]])->getName()
+ << ")" << std::endl;
+ }
+ }
+ sotDEBUGOUT(25);
+}
+
+void MatrixInertia::
+initDofTable( void )
+{
+ sotDEBUGIN(25);
+ for(size_t i = 0; i < joints_.size(); ++i)
+ {
+ Joint* j = static_cast<Joint*>(joints_[i]);
+ vector3d z = j->axe();
+ ml::Vector & phi_i = phi[i];
+ phi_i.resize(6); phi_i.fill(0.);
+ for( int n=0;n<3;++n ) phi_i(n+3) = z(n);
+ sotDEBUG(25) << phi_i <<endl;
+ }
+ sotDEBUGOUT(25);
+}
+
+MatrixInertia::~MatrixInertia()
+{}
+
+/* -------------------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+void MatrixInertia::update( void )
+{
+ sotDEBUGIN(25);
+ const vectorN & currentConf = aHDMB_->currentConfiguration();
+ sotDEBUG(45) << "q = [ " << currentConf << endl;
+
+ sotDEBUG(15) << "Spatial transforms:" << std::endl;
+ const size_t SIZE = joints_.size();
+ for(size_t i = 0; i < SIZE; ++i)
+ {
+ sotDEBUG(25) << "Joint " << i << ": rank = "
+ << joints_[i]->rankInConfiguration() <<endl;
+ Joint* j = static_cast<Joint*>(joints_[i]);
+
+ /* iRpi: Rotation from joint i to parent of i. */
+ MatrixRotation piRi;
+ {
+ /* phi_[1] = phi_(1:3) is the rotation axis of the joint. */
+ matrix3d Ri; j->getStaticRotation(Ri);
+ vector3d axeJ = Ri*j->axe();
+ const double & q = currentConf(joints_[i]->rankInConfiguration());
+ sotDEBUG(45) << "q"<<i<<" = " <<q <<endl;
+ matrix3d piRi_tmp;
+ j->RodriguesRotation(axeJ,q,piRi_tmp);
+ for( unsigned int loopi=0;loopi<3;++loopi )
+ for( unsigned int loopj=0;loopj<3;++loopj )
+ piRi( loopi,loopj ) = piRi_tmp(loopi,loopj);
+ }
+ ml::Vector piTi(3);
+ {
+ vector3d piTi_tmp;
+ j->getStaticTranslation( piTi_tmp );
+ for( unsigned int loopi=0;loopi<3;++loopi ) piTi(loopi) = piTi_tmp(loopi);
+ }
+ /* Twist matrix iXpi: transfo of the velocities between i en pi (parent i).
+ * iXpi = [ iRpi Skew( iRpi.t )*iRpi ]
+ * [ 0 iRpi ] */
+ MatrixHomogeneous piMi; piMi.buildFrom( piRi,piTi );
+ MatrixHomogeneous iMpi; piMi.inverse(iMpi);
+ sotDEBUG(45) << "piMi = " <<piMi <<endl;
+ sotDEBUG(45) << "iMpi = " <<iMpi <<endl;
+
+ iVpi[i].buildFrom( iMpi );
+ iVpi[i].transpose( iVpiT[i] );
+ sotDEBUG(25) << "iVpi" << i << " = " <<iVpi[i] <<endl;
+ }
+ sotDEBUGOUT(25);
+}
+
+void MatrixInertia::computeInertiaMatrix()
+{
+ sotDEBUGIN(25);
+ inertia_.fill(0.0);
+
+ const size_t SIZE = joints_.size();
+
+ /* Compute the local 6D inertia matrices. */
+ for( size_t i = 0;i<SIZE;++i )
+ {
+ /* Position of the mass in the joint frame. */
+ vector3d com = joints_[i]->linkedBody()->localCenterOfMass();
+ matrix3d Sc = skewSymmetric(com);
+ /* Inertia of the link. */
+ matrix3d Icm = joints_[i]->linkedBody()->inertiaMatrix();
+
+ double m = joints_[i]->linkedBody()->mass();
+
+
+ /* Ic_ is the inertia 6D matrix of the joint in the joint frame.
+ * Ic = [ Ai+mSc.Sc' mSc ]
+ * [ mSc' mId ]
+ */
+ sotDEBUG(45) << "com"<<i<<" = [ " << com <<"]"<<endl;
+ sotDEBUG(45) << "Sc"<<i<<" = [ " << Sc<<"]"<<endl;
+ sotDEBUG(45) << "Icm"<<i<<" = [ " << Icm<<"]"<<endl;
+ matrix3d Sct = Sc.Transpose();
+ matrix3d Irr = Sc*Sct;
+ ml::Matrix & Ici = Ic[i]; Ici.resize(6,6);
+ for( unsigned int loopi=0;loopi<3;++loopi )
+ for( unsigned int loopj=0;loopj<3;++loopj )
+ {
+ /*TT*/if( loopi==loopj ) Ici( loopi,loopj ) = m;
+ else Ici( loopi,loopj ) = 0.;
+ /*TR*/Ici( loopi,loopj+3 ) = m*Sct( loopi,loopj );
+ /*RT*/Ici( loopi+3,loopj ) = m*Sc( loopi,loopj );
+ /*RR*/Ici( loopi+3,loopj+3 ) = m*Irr( loopi,loopj )+ Icm( loopi,loopj );
+ }
+
+ sotDEBUG(25) << "Ic" << i << " = " << Ici;
+ }
+
+ ml::Vector Fi(6);
+ ml::Matrix iVpiT_Ici(6,6);
+ ml::Matrix iVpiT_Ici_iVpi(6,6);
+
+ for( int i=SIZE-1;i>=1;--i )
+ {
+ const unsigned int iRank = joints_[i]->rankInConfiguration();
+
+ ml::Matrix & Ici = Ic[i];
+ sotMatrixTwist & iVpii = iVpi[i];
+ MatrixForce & iVpiiT = iVpiT[i];
+ ml::Vector & phii = phi[i];
+ /* F = Ic_i . phi_i */
+ Fi = Ici*phii;
+ /* H_ii = phi_i' . F */
+ /*DEBUGinertia_(i + 5, i + 5) = phii.scalarProduct(Fi);*/
+
+
+ inertia_(iRank,iRank) = phii.scalarProduct(Fi);
+ sotDEBUG(30) << "phi"<<i<<" = " << phii;
+ sotDEBUG(35) << "Fi"<<i<<" = " << Fi << endl;
+ sotDEBUG(25) << "IcA"<<i<<" = " << Ici << endl;
+ sotDEBUG(45) << "Joint " << i << " in " << iRank <<endl;
+
+ /* Ic_pi = Ic_pi + iXpi' Ic_i iXpi */
+ iVpiiT.multiply(Ici,iVpiT_Ici);
+ iVpiT_Ici.multiply( iVpii,iVpiT_Ici_iVpi );
+
+ sotDEBUG(45) << "Icpi"<<parentIndex_[i]<<" = " << Ic[parentIndex_[i]] ;
+ Ic[ parentIndex_[i] ] += iVpiT_Ici_iVpi;
+
+ sotDEBUG(45) << "Icpi"<<parentIndex_[i]<<"_"<<i<<" = " << Ic[parentIndex_[i]] ;
+ sotDEBUG(45) << "Vpi"<<i<<" = " << iVpii ;
+
+ size_t j = i;
+ while(parentIndex_[j] != 0)
+ {
+ /* F = jXpj' . F */
+ Fi = iVpiT[j]*Fi;
+ /* j = pj */
+ j = parentIndex_[j];
+ /* Hij = Hji = F' phi_j */
+ inertia_(iRank,joints_[j]->rankInConfiguration())
+ = inertia_(joints_[j]->rankInConfiguration(),iRank)
+ = Fi.scalarProduct( phi[j]);
+ sotDEBUG(35) << "Fi = " << Fi << endl;
+ sotDEBUG(35) << "FiXphi = " << inertia_(j + 5, i + 5) << endl;
+ }
+
+ /* When parentIndex_[j] == 0: FREE FLYER. */
+ Fi = iVpiT[j]*Fi;
+ for(size_t k = 0; k < 6; ++k)
+ {
+ inertia_(iRank, k) = inertia_(k,iRank) = Fi(k);
+ }
+ }
+
+ /* --- FREE FLYER = Ic0 --- */
+ const ml::Matrix & Ic0 = Ic[0];
+ for(size_t i = 0; i < 6; ++i)
+ for(size_t j = 0; j < 6; ++j)
+ {inertia_(i, j) = Ic0(i,j);}
+
+
+ sotDEBUGOUT(25);
+}
+
+/* -------------------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+
+
+void MatrixInertia::
+getInertiaMatrix(double* A)
+{
+ for(size_t i = 0; i < (joints_.size() + 5); ++i)
+ for(size_t j = 0; j < (joints_.size() + 5); ++j)
+ A[i * (joints_.size() + 5) + j] = inertia_(i, j);
+}
+
+const maal::boost::Matrix& MatrixInertia::
+getInertiaMatrix( void )
+{ return inertia_; }
diff --git a/src/waist-attitude-from-sensor.cpp b/src/waist-attitude-from-sensor.cpp
new file mode 100644
index 0000000..ced2107
--- /dev/null
+++ b/src/waist-attitude-from-sensor.cpp
@@ -0,0 +1,200 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: WaistAttitudeFromSensor.h
+ * Project: SOT
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+#include <sot-dynamic/waist-attitude-from-sensor.h>
+#include <sot-core/debug.h>
+#include <dynamic-graph/factory.h>
+
+using namespace sot;
+using namespace dynamicgraph;
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(WaistAttitudeFromSensor,"WaistAttitudeFromSensor");
+
+WaistAttitudeFromSensor::
+WaistAttitudeFromSensor( const std::string & name )
+ :Entity(name)
+ ,attitudeSensorSIN(NULL,"sotWaistAttitudeFromSensor("+name+")::input(MatrixRotation)::attitudeIN")
+ ,positionSensorSIN(NULL,"sotWaistAttitudeFromSensor("+name+")::input(matrixHomo)::position")
+ ,attitudeWaistSOUT( boost::bind(&WaistAttitudeFromSensor::computeAttitudeWaist,this,_1,_2),
+ attitudeSensorSIN<<positionSensorSIN,
+ "sotWaistAttitudeFromSensor("+name+")::output(RPY)::attitude" )
+{
+ sotDEBUGIN(5);
+
+ signalRegistration( attitudeSensorSIN<<positionSensorSIN
+ <<attitudeWaistSOUT );
+
+ sotDEBUGOUT(5);
+}
+
+
+WaistAttitudeFromSensor::
+~WaistAttitudeFromSensor( void )
+{
+ sotDEBUGIN(5);
+
+ sotDEBUGOUT(5);
+ return;
+}
+
+/* --- SIGNALS -------------------------------------------------------------- */
+/* --- SIGNALS -------------------------------------------------------------- */
+/* --- SIGNALS -------------------------------------------------------------- */
+VectorRollPitchYaw & WaistAttitudeFromSensor::
+computeAttitudeWaist( VectorRollPitchYaw & res,
+ const int& time )
+{
+ sotDEBUGIN(15);
+
+ const MatrixHomogeneous & waistMchest = positionSensorSIN( time );
+ const MatrixRotation & worldRchest = attitudeSensorSIN( time );
+
+ MatrixRotation waistRchest; waistMchest.extract(waistRchest);
+ MatrixRotation chestRwaist; waistRchest.transpose( chestRwaist );
+
+ MatrixRotation worldrchest;
+ worldRchest.multiply( chestRwaist,worldrchest);
+ res.fromMatrix(worldrchest);
+ sotDEBUGOUT(15);
+ return res;
+}
+
+
+
+/* --- PARAMS --------------------------------------------------------------- */
+/* --- PARAMS --------------------------------------------------------------- */
+/* --- PARAMS --------------------------------------------------------------- */
+void WaistAttitudeFromSensor::
+commandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os )
+{
+ sotDEBUG(25) << "Cmd " << cmdLine <<std::endl;
+
+ if( cmdLine == "help" )
+ {
+ Entity::commandLine(cmdLine,cmdArgs,os);
+ }
+ else { Entity::commandLine( cmdLine,cmdArgs,os); }
+}
+
+
+/* === WaistPoseFromSensorAndContact ===================================== */
+/* === WaistPoseFromSensorAndContact ===================================== */
+/* === WaistPoseFromSensorAndContact ===================================== */
+
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(WaistPoseFromSensorAndContact,
+ "WaistPoseFromSensorAndContact");
+
+WaistPoseFromSensorAndContact::
+WaistPoseFromSensorAndContact( const std::string & name )
+ :WaistAttitudeFromSensor(name)
+ ,fromSensor(false)
+ ,positionContactSIN(NULL,"sotWaistPoseFromSensorAndContact("+name+")::input(matrixHomo)::contact")
+ ,positionWaistSOUT( boost::bind(&WaistPoseFromSensorAndContact::computePositionWaist,this,_1,_2),
+ attitudeWaistSOUT<<positionContactSIN,
+ "sotWaistPoseFromSensorAndContact("+name+")::output(RPY+T)::positionWaist" )
+{
+ sotDEBUGIN(5);
+
+ signalRegistration( positionContactSIN<<positionWaistSOUT );
+
+ sotDEBUGOUT(5);
+}
+
+
+WaistPoseFromSensorAndContact::
+~WaistPoseFromSensorAndContact( void )
+{
+ sotDEBUGIN(5);
+
+ sotDEBUGOUT(5);
+ return;
+}
+
+/* --- SIGNALS -------------------------------------------------------------- */
+/* --- SIGNALS -------------------------------------------------------------- */
+/* --- SIGNALS -------------------------------------------------------------- */
+ml::Vector& WaistPoseFromSensorAndContact::
+computePositionWaist( ml::Vector& res,
+ const int& time )
+{
+ sotDEBUGIN(15);
+
+ const MatrixHomogeneous& waistMcontact = positionContactSIN( time );
+ MatrixHomogeneous contactMwaist; waistMcontact.inverse(contactMwaist);
+
+ res.resize(6);
+ for( unsigned int i=0;i<3;++i )
+ { res(i)=contactMwaist(i,3); }
+
+ if(fromSensor)
+ {
+ const VectorRollPitchYaw & worldrwaist = attitudeWaistSOUT( time );
+ for( unsigned int i=0;i<3;++i )
+ { res(i+3)=worldrwaist(i); }
+ }
+ else
+ {
+ MatrixRotation contactRwaist;
+ contactMwaist.extract(contactRwaist);
+ VectorRollPitchYaw contactrwaist;
+ contactrwaist.fromMatrix( contactRwaist );
+
+ for( unsigned int i=0;i<3;++i )
+ { res(i+3)=contactrwaist(i); }
+ }
+
+
+
+
+ sotDEBUGOUT(15);
+ return res;
+}
+
+
+
+/* --- PARAMS --------------------------------------------------------------- */
+/* --- PARAMS --------------------------------------------------------------- */
+/* --- PARAMS --------------------------------------------------------------- */
+void WaistPoseFromSensorAndContact::
+commandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os )
+{
+ sotDEBUG(25) << "Cmd " << cmdLine <<std::endl;
+
+ if( cmdLine == "help" )
+ {
+ os <<"WaistPoseFromSensorAndContact:"<<std::endl
+ <<" - fromSensor [true|false]: get/set the flag." << std::endl;
+ WaistAttitudeFromSensor::commandLine(cmdLine,cmdArgs,os);
+ }
+ else if( cmdLine == "fromSensor" )
+ {
+ std::string val; cmdArgs>>val;
+ if( ("true"==val)||("false"==val) )
+ {
+ fromSensor = ( val=="true" );
+ } else {
+ os << "fromSensor = " << (fromSensor?"true":"false") << std::endl;
+ }
+ }
+ else { WaistAttitudeFromSensor::commandLine( cmdLine,cmdArgs,os); }
+}
+
diff --git a/src/zmpreffromcom.cpp b/src/zmpreffromcom.cpp
new file mode 100644
index 0000000..c60d6b4
--- /dev/null
+++ b/src/zmpreffromcom.cpp
@@ -0,0 +1,119 @@
+/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ * Copyright Projet JRL-Japan, 2007
+ *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * File: ZmprefFromCom.h
+ * Project: SOT
+ * Author: Nicolas Mansard
+ *
+ * Version control
+ * ===============
+ *
+ * $Id$
+ *
+ * Description
+ * ============
+ *
+ *
+ * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+
+#include <sot-dynamic/zmpreffromcom.h>
+#include <sot-core/debug.h>
+#include <dynamic-graph/factory.h>
+
+using namespace sot;
+using namespace dynamicgraph;
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ZmprefFromCom,"ZmprefFromCom");
+
+
+const double ZmprefFromCom::DT_DEFAULT = 5e-3;
+const double ZmprefFromCom::FOOT_HEIGHT_DEFAULT = .105;
+
+ZmprefFromCom::
+ZmprefFromCom( const std::string & name )
+ :Entity(name)
+ ,dt(DT_DEFAULT)
+ ,footHeight(FOOT_HEIGHT_DEFAULT)
+ ,waistPositionSIN(NULL,"sotZmprefFromCom("+name+")::input(MatrixHomo)::waist")
+ ,comPositionSIN(NULL,"sotZmprefFromCom("+name+")::input(Vector)::com")
+ ,dcomSIN(NULL,"sotZmprefFromCom("+name+")::input(Vector)::dcom")
+ ,zmprefSOUT( boost::bind(&ZmprefFromCom::computeZmpref,this,_1,_2),
+ waistPositionSIN<<comPositionSIN<<dcomSIN,
+ "sotZmprefFromCom("+name+")::output(RPY)::zmpref" )
+{
+ sotDEBUGIN(5);
+
+ signalRegistration(waistPositionSIN<<comPositionSIN<<dcomSIN<<zmprefSOUT );
+
+ sotDEBUGOUT(5);
+}
+
+
+ZmprefFromCom::
+~ZmprefFromCom( void )
+{
+ sotDEBUGIN(5);
+
+ sotDEBUGOUT(5);
+ return;
+}
+
+/* --- SIGNALS -------------------------------------------------------------- */
+/* --- SIGNALS -------------------------------------------------------------- */
+/* --- SIGNALS -------------------------------------------------------------- */
+ml::Vector& ZmprefFromCom::
+computeZmpref( ml::Vector& res,
+ const int& time )
+{
+ sotDEBUGIN(15);
+
+ const ml::Vector& com = comPositionSIN( time );
+ const ml::Vector& dcom = dcomSIN( time );
+ const MatrixHomogeneous& oTw = waistPositionSIN( time );
+
+ MatrixHomogeneous wTo = oTw.inverse();
+ ml::Vector nextComRef = dcom; nextComRef*= dt;nextComRef+=com;
+
+
+ nextComRef(2) = -footHeight; // projection on the ground.
+ wTo.multiply(nextComRef,res);
+
+ sotDEBUGOUT(15);
+ return res;
+}
+
+
+
+/* --- PARAMS --------------------------------------------------------------- */
+/* --- PARAMS --------------------------------------------------------------- */
+/* --- PARAMS --------------------------------------------------------------- */
+void ZmprefFromCom::
+commandLine( const std::string& cmdLine,
+ std::istringstream& cmdArgs,
+ std::ostream& os )
+{
+ sotDEBUG(25) << "Cmd " << cmdLine <<std::endl;
+
+ if( cmdLine == "help" )
+ {
+ os<<"zmprefFromCom:" << std::endl
+ <<" - dt [<value>]: get/set the dt value." << std::endl
+ <<" - footHeight [<value>]: "
+ <<"get/set the height of the foot ("
+ << FOOT_HEIGHT_DEFAULT <<" by default)." <<std::endl;
+ Entity::commandLine(cmdLine,cmdArgs,os);
+ }
+ if( cmdLine == "dt" )
+ {
+ cmdArgs>>std::ws;
+ if( cmdArgs.good() ) cmdArgs>>dt; else os <<"dt = " << dt<<std::endl;
+ }
+ if( cmdLine == "footHeight" )
+ {
+ cmdArgs>>std::ws;
+ if( cmdArgs.good() ) cmdArgs>>footHeight;
+ else os <<"footHeight = " << footHeight<<std::endl;
+ }
+ else { Entity::commandLine( cmdLine,cmdArgs,os); }
+}
+
diff --git a/unitTesting/CMakeLists.txt b/unitTesting/CMakeLists.txt
new file mode 100644
index 0000000..8221c67
--- /dev/null
+++ b/unitTesting/CMakeLists.txt
@@ -0,0 +1,46 @@
+#
+# Copyright
+#
+
+SET(EXECUTABLE_NAME test${PROJECT_NAME})
+
+ADD_DEFINITIONS(-DDEBUG=2)
+
+# provide path to library libdynamic-graph.so
+LINK_DIRECTORIES(${DYNAMIC_GRAPH_LIBRARY_DIRS})
+
+# provide path to library libsot-core.so
+LINK_DIRECTORIES(${SOT_CORE_LIBRARY_DIRS})
+
+ADD_EXECUTABLE(${EXECUTABLE_NAME}
+ dummy.cpp)
+
+INCLUDE_DIRECTORIES(${CMAKE_SOURCE_DIR}/include)
+
+LINK_DIRECTORIES(${${PROJECT_NAME}_BINARY_DIR}/src)
+TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME}
+ zmpreffromcom
+ force-compensation
+ integrator-force-exact
+ mass-apparent
+ integrator-force-rk4
+ integrator-force
+ angle-estimator
+ waist-attitude-from-sensor)
+
+ IF (UNIX)
+ TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${DYNAMIC_GRAPH_LIBRARIES})
+ TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${SOT_CORE_LIBRARIES})
+ TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${MATRIXABSTRACTLAYER_LIBRARIES})
+ TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${HRP2DYNAMICS_LIBRARIES})
+ TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${DYNAMICSJRLJAPAN_LIBRARIES})
+ ENDIF(UNIX)
+
+ IF(WIN32)
+ SET_TARGET_PROPERTIES(${EXECUTABLE_NAME}
+ PROPERTIES
+ LINK_FLAGS "${${PROJECT_NAME}_src_LDFLAGS}"
+ )
+ ENDIF(WIN32)
+
+ADD_TEST(dummy ${EXECUTABLE_NAME})
\ No newline at end of file
diff --git a/unitTesting/dummy.cpp b/unitTesting/dummy.cpp
new file mode 100644
index 0000000..796daa0
--- /dev/null
+++ b/unitTesting/dummy.cpp
@@ -0,0 +1,8 @@
+/*
+ * Copyright
+ */
+
+int main (int argc, char** argv)
+{
+
+}
--
GitLab