IVIGIT.

AUTHORS

+This package was written by and with the assistance of:
+
+* Nicolas Mansard       nicolas.mansard@laas.fr
+* Layale Saab           lsaab@laas.fr
+* Olivier Stasse	olivier.stasse@aist.go.jp

CMakeLists.txt

+# Copyright 2010, Nicolas Mansard, LAAS-CNRS
+#
+# This file is part of sot-dyninv.
+# sot-dyninv is free software: you can redistribute it and/or
+# modify it under the terms of the GNU Lesser General Public License
+# as published by the Free Software Foundation, either version 3 of
+# the License, or (at your option) any later version.
+#
+# sot-dyninv is distributed in the hope that it will be useful, but
+# WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+# General Lesser Public License for more details.  You should have
+# received a copy of the GNU Lesser General Public License along with
+# sot-dyninv. If not, see <http://www.gnu.org/licenses/>.
+
+CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+
+INCLUDE(cmake/base.cmake)
+INCLUDE(cmake/boost.cmake)
+INCLUDE(cmake/lapack.cmake)
+INCLUDE(cmake/cpack.cmake)
+
+SET(PROJECT_NAME sot-dyninv)
+SET(PROJECT_DESCRIPTION "control by inverse dynamics.")
+SET(PROJECT_URL "http://github.com/laas/sot-dyninv")
+
+SET(CUSTOM_HEADER_DIR "${PROJECT_NAME}")
+
+# Disable -Werror on Unix for now.
+SET(CXX_DISABLE_WERROR True)
+
+SET(PKG_CONFIG_ADDITIONAL_VARIABLES
+  ${PKG_CONFIG_ADDITIONAL_VARIABLES}
+  plugindirname
+  plugindir
+  )
+
+SETUP_PROJECT()
+
+# Search for dependencies.
+ADD_REQUIRED_DEPENDENCY("jrl-mal >= 1.8.0")
+ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 1.0.0")
+ADD_REQUIRED_DEPENDENCY("sot-core >= 1.0.0")
+
+# List plug-ins that will be compiled.
+SET(libs
+  controller-pd
+  )
+
+LIST(APPEND LOGGING_WATCHED_TARGETS ${libs})
+
+# Add subdirectories.
+ADD_SUBDIRECTORY(include)
+ADD_SUBDIRECTORY(src)
+ADD_SUBDIRECTORY(unitTesting)
+
+SETUP_PROJECT_FINALIZE()
+SETUP_PROJECT_CPACK()
\ No newline at end of file

COPYING.LESSER

+                   GNU LESSER GENERAL PUBLIC LICENSE
+                       Version 3, 29 June 2007
+
+ Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+
+  This version of the GNU Lesser General Public License incorporates
+the terms and conditions of version 3 of the GNU General Public
+License, supplemented by the additional permissions listed below.
+
+  0. Additional Definitions.
+
+  As used herein, "this License" refers to version 3 of the GNU Lesser
+General Public License, and the "GNU GPL" refers to version 3 of the GNU
+General Public License.
+
+  "The Library" refers to a covered work governed by this License,
+other than an Application or a Combined Work as defined below.
+
+  An "Application" is any work that makes use of an interface provided
+by the Library, but which is not otherwise based on the Library.
+Defining a subclass of a class defined by the Library is deemed a mode
+of using an interface provided by the Library.
+
+  A "Combined Work" is a work produced by combining or linking an
+Application with the Library.  The particular version of the Library
+with which the Combined Work was made is also called the "Linked
+Version".
+
+  The "Minimal Corresponding Source" for a Combined Work means the
+Corresponding Source for the Combined Work, excluding any source code
+for portions of the Combined Work that, considered in isolation, are
+based on the Application, and not on the Linked Version.
+
+  The "Corresponding Application Code" for a Combined Work means the
+object code and/or source code for the Application, including any data
+and utility programs needed for reproducing the Combined Work from the
+Application, but excluding the System Libraries of the Combined Work.
+
+  1. Exception to Section 3 of the GNU GPL.
+
+  You may convey a covered work under sections 3 and 4 of this License
+without being bound by section 3 of the GNU GPL.
+
+  2. Conveying Modified Versions.
+
+  If you modify a copy of the Library, and, in your modifications, a
+facility refers to a function or data to be supplied by an Application
+that uses the facility (other than as an argument passed when the
+facility is invoked), then you may convey a copy of the modified
+version:
+
+   a) under this License, provided that you make a good faith effort to
+   ensure that, in the event an Application does not supply the
+   function or data, the facility still operates, and performs
+   whatever part of its purpose remains meaningful, or
+
+   b) under the GNU GPL, with none of the additional permissions of
+   this License applicable to that copy.
+
+  3. Object Code Incorporating Material from Library Header Files.
+
+  The object code form of an Application may incorporate material from
+a header file that is part of the Library.  You may convey such object
+code under terms of your choice, provided that, if the incorporated
+material is not limited to numerical parameters, data structure
+layouts and accessors, or small macros, inline functions and templates
+(ten or fewer lines in length), you do both of the following:
+
+   a) Give prominent notice with each copy of the object code that the
+   Library is used in it and that the Library and its use are
+   covered by this License.
+
+   b) Accompany the object code with a copy of the GNU GPL and this license
+   document.
+
+  4. Combined Works.
+
+  You may convey a Combined Work under terms of your choice that,
+taken together, effectively do not restrict modification of the
+portions of the Library contained in the Combined Work and reverse
+engineering for debugging such modifications, if you also do each of
+the following:
+
+   a) Give prominent notice with each copy of the Combined Work that
+   the Library is used in it and that the Library and its use are
+   covered by this License.
+
+   b) Accompany the Combined Work with a copy of the GNU GPL and this license
+   document.
+
+   c) For a Combined Work that displays copyright notices during
+   execution, include the copyright notice for the Library among
+   these notices, as well as a reference directing the user to the
+   copies of the GNU GPL and this license document.
+
+   d) Do one of the following:
+
+       0) Convey the Minimal Corresponding Source under the terms of this
+       License, and the Corresponding Application Code in a form
+       suitable for, and under terms that permit, the user to
+       recombine or relink the Application with a modified version of
+       the Linked Version to produce a modified Combined Work, in the
+       manner specified by section 6 of the GNU GPL for conveying
+       Corresponding Source.
+
+       1) Use a suitable shared library mechanism for linking with the
+       Library.  A suitable mechanism is one that (a) uses at run time
+       a copy of the Library already present on the user's computer
+       system, and (b) will operate properly with a modified version
+       of the Library that is interface-compatible with the Linked
+       Version.
+
+   e) Provide Installation Information, but only if you would otherwise
+   be required to provide such information under section 6 of the
+   GNU GPL, and only to the extent that such information is
+   necessary to install and execute a modified version of the
+   Combined Work produced by recombining or relinking the
+   Application with a modified version of the Linked Version. (If
+   you use option 4d0, the Installation Information must accompany
+   the Minimal Corresponding Source and Corresponding Application
+   Code. If you use option 4d1, you must provide the Installation
+   Information in the manner specified by section 6 of the GNU GPL
+   for conveying Corresponding Source.)
+
+  5. Combined Libraries.
+
+  You may place library facilities that are a work based on the
+Library side by side in a single library together with other library
+facilities that are not Applications and are not covered by this
+License, and convey such a combined library under terms of your
+choice, if you do both of the following:
+
+   a) Accompany the combined library with a copy of the same work based
+   on the Library, uncombined with any other library facilities,
+   conveyed under the terms of this License.
+
+   b) Give prominent notice with the combined library that part of it
+   is a work based on the Library, and explaining where to find the
+   accompanying uncombined form of the same work.
+
+  6. Revised Versions of the GNU Lesser General Public License.
+
+  The Free Software Foundation may publish revised and/or new versions
+of the GNU Lesser General Public License from time to time. Such new
+versions will be similar in spirit to the present version, but may
+differ in detail to address new problems or concerns.
+
+  Each version is given a distinguishing version number. If the
+Library as you received it specifies that a certain numbered version
+of the GNU Lesser General Public License "or any later version"
+applies to it, you have the option of following the terms and
+conditions either of that published version or of any later version
+published by the Free Software Foundation. If the Library as you
+received it does not specify a version number of the GNU Lesser
+General Public License, you may choose any version of the GNU Lesser
+General Public License ever published by the Free Software Foundation.
+
+  If the Library as you received it specifies that a proxy can decide
+whether future versions of the GNU Lesser General Public License shall
+apply, that proxy's public statement of acceptance of any version is
+permanent authorization for you to choose that version for the
+Library.

README.md

+sot-dyninv
+==========
+
+This sofware provides the solver and features to resolve the inverse of the
+dynamics of a free-floating robot, in contact with its environment.  The basics
+are provided by the sot core, and the computation of the dynamics of the robot
+can be computed by a third party lib, for example sot-dynamic.
+
+
+Setup
+-----
+
+To compile this package, it is recommended to create a separate build
+directory:
+
+    mkdir _build
+    cd _build
+    cmake [OPTIONS] ..
+    make install
+
+Please note that CMake produces a `CMakeCache.txt` file which should
+be deleted to reconfigure a package from scratch.
+
+
+### Dependencies
+
+The sot dynamic inverse depends on several packages which
+have to be available on your machine.
+
+ - Libraries:
+   - sot-core (>= 1.0.0)
+ - System tools:
+   - CMake (>=2.6)
+   - pkg-config
+   - usual compilation tools (GCC/G++, make, etc.)
+
+
+[sot-core]: http://github.com/jrl-umi3128/sot-core

cmake_uninstall.cmake.in

+# Template for uninstalling the files
+# provided by this package.
+#
+# This file is coming straight from
+# http://www.cmake.org/Wiki/CMake_FAQ
+# O. Stasse, JRL, CNRS, 2010
+#
+
+IF(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
+  MESSAGE(FATAL_ERROR "Cannot find install manifest: \"@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt\"")
+ENDIF(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
+
+FILE(READ "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt" files)
+STRING(REGEX REPLACE "\n" ";" files "${files}")
+FOREACH(file ${files})
+  MESSAGE(STATUS "Uninstalling \"$ENV{DESTDIR}${file}\"")
+  IF(EXISTS "$ENV{DESTDIR}${file}")
+    EXEC_PROGRAM(
+      "@CMAKE_COMMAND@" ARGS "-E remove \"$ENV{DESTDIR}${file}\""
+      OUTPUT_VARIABLE rm_out
+      RETURN_VALUE rm_retval
+      )
+    IF(NOT "${rm_retval}" STREQUAL 0)
+      MESSAGE(FATAL_ERROR "Problem when removing \"$ENV{DESTDIR}${file}\"")
+    ENDIF(NOT "${rm_retval}" STREQUAL 0)
+  ELSE(EXISTS "$ENV{DESTDIR}${file}")
+    MESSAGE(STATUS "File \"$ENV{DESTDIR}${file}\" does not exist.")
+  ENDIF(EXISTS "$ENV{DESTDIR}${file}")
+ENDFOREACH(file)

doc/CMakeLists.txt

+#
+#  Copyright 2011 CNRS
+#  Author: Nicolas Mansard (empty doc file ... who else?)
+#
+

doc/Doxyfile.extra.in

+INPUT                  = @CMAKE_SOURCE_DIR@/include \
+                         @CMAKE_SOURCE_DIR@/doc/additionalDoc
+IMAGE_PATH             = @CMAKE_SOURCE_DIR@/doc/pictures
+
+FILE_PATTERNS          = *.cc *.cpp *.h *.hh *.hxx
+
+TAGFILES               = \
+"@JRL_MAL_DOXYGENDOCDIR@/jrl-mal.doxytag = @JRL_MAL_DOXYGENDOCDIR@" \
+"@DYNAMIC_GRAPH_DOXYGENDOCDIR@/dynamic-graph.doxytag = @DYNAMIC_GRAPH_DOXYGENDOCDIR@" \
+"@SOT_CORE_DOXYGENDOCDIR@/sot-core.doxytag = @SOT_CORE_DOXYGENDOCDIR@"

doc/additionalDoc/package.hh

+/*
+ * Copyright 2010,
+ * François Bleibel,
+ * Olivier Stasse,
+ *
+ * CNRS/AIST
+ *
+ * This file is part of sot-dynamic.
+ * sot-dynamic is free software: you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public License
+ * as published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ * sot-dynamic is distributed in the hope that it will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License for more details.  You should
+ * have received a copy of the GNU Lesser General Public License along
+ * with sot-dynamic.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/** \mainpage
+\section sot_dynamic_section_introduction Introduction
+
+The sot-dynamic package is a bridge between the stack of tasks framework and the dynamicsJRLJapan library.
+It provides an inverse dynamic model of the robot through dynamic-graph entities.
+More precisely it wraps the newton euler algorithm implemented by the dynamicsJRLJapan library
+to make it accessible for the stack of tasks controllers
+(in the Stack of Tasks Framework as defined in \ref Mansard2007.)
+
+This package depends on the following packages:
+\li dynamicsJRLJapan
+\li sot-core
+\li dynamic-graph
+\li dynamic-graph-python
+Optional packages (for specific support of the hrp2-N robots)
+\li hrp210optimized
+\li hrp2-dynamics
+
+See the JRL umi3218's page on github for instructions on how to download and
+install these packages at https://github.com/jrl-umi3218.
+
+\section python_bindings Python bindings
+
+As most packages based on the dynamic-graph framework (see https://github.com/jrl-umi3218/dynamic-graph),
+the functionnality is exposed through entities. Hence python sub-modules of dynamic_graph  are generated. See <a href="../sphinx-html/index.html">sphinx documentation</a> for more details.
+
+The following entities are created by this package:\n
+(all entites are placed in the namespace sot::)
+\li sot::ZmprefFromCom
+\li sot::ForceCompensation
+\li sot::IntegratorForceExact
+\li sot::MassApparent
+\li sot::IntegratorForceRk4
+\li sot::IntegratorForce
+\li sot::AngleEstimator
+\li sot::WaistAttitudeFromSensor
+\li sot::Dynamic - provides the inverse dynamics computations for of a humanoid robot
+
+Optionally, if the packages in brackets are installed, the following entities
+are made available:
+\li sot::DynamicHrp2 - same as sot::Dynamic, but specialized for hrp2 robots [needs hrp2-dynamics]
+\li sot::DynamicHrp2_10 - same as previous, optimized for the hrp2-10 robot [needs hrp210optimized]
+\li sot::DynamicHrp2_10_old [needs hrp210optimized]
+
+See each entity's documentation page for more information (when available).
+
+\section References
+\anchor Mansard2007
+
+<b>"Task sequencing for sensor-based control"</b>,
+<em>N. Mansard, F. Chaumette,</em>
+IEEE Trans. on Robotics, 23(1):60-72, February 2007
+
+**/

doc/pictures/footer.txt

+Copy in this directory the image you wish to use in the footer of the documentation or edit file sot-core/doc/footer.html and
+remove this file.

include/CMakeLists.txt

+# Copyright 2011, Nicolas Mansard, LAAS-CNRS
+#
+# This file is part of sot-dyninv.
+# sot-dyninv is free software: you can redistribute it and/or
+# modify it under the terms of the GNU Lesser General Public License
+# as published by the Free Software Foundation, either version 3 of
+# the License, or (at your option) any later version.
+#
+# sot-dyninv is distributed in the hope that it will be useful, but
+# WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+# General Lesser Public License for more details.  You should have
+# received a copy of the GNU Lesser General Public License along with
+# sot-dyninv. If not, see <http://www.gnu.org/licenses/>.
+
+SET(${PROJECT_NAME}_HEADERS
+	controller-pd.h
+)
+
+# Recreate correct path for the headers
+#--------------------------------------
+
+SET(fullpath_${PROJECT_NAME}_HEADERS)
+FOREACH(lHeader ${${PROJECT_NAME}_HEADERS})
+  SET(fullpath_${PROJECT_NAME}_HEADERS
+    ${fullpath_${PROJECT_NAME}_HEADERS}
+    ./${PROJECT_NAME}/${lHeader}
+  )
+
+ENDFOREACH(lHeader)
+
+#----------------------------------------------------
+# Install procedure for the header files
+#----------------------------------------------------
+INSTALL(FILES ${fullpath_${PROJECT_NAME}_HEADERS}
+	DESTINATION include/${PROJECT_NAME}
+        PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE
+)

include/sot-dyninv/controller-pd.h

+/*
+ * Copyright 2011, Nicolas Mansard, LAAS-CNRS
+ *
+ * This file is part of sot-dyninv.
+ * sot-dyninv is free software: you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public License
+ * as published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ * sot-dyninv is distributed in the hope that it will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License for more details.  You should
+ * have received a copy of the GNU Lesser General Public License along
+ * with sot-dyninv.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __sot_dyninv_ControllerPD_H__
+#define __sot_dyninv_ControllerPD_H__
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined (WIN32)
+#  if defined (controller_pd_EXPORTS)
+#    define SOTCONTROLLERPD_EXPORT __declspec(dllexport)
+#  else
+#    define SOTCONTROLLERPD_EXPORT __declspec(dllimport)
+#  endif
+#else
+#  define SOTCONTROLLERPD_EXPORT
+#endif
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/* Matrix */
+#include <jrl/mal/boost.hh>
+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 dyninv {
+
+    namespace dg = dynamicgraph;
+
+  /* --------------------------------------------------------------------- */
+  /* --- CLASS ----------------------------------------------------------- */
+  /* --------------------------------------------------------------------- */
+
+  class SOTCONTROLLERPD_EXPORT ControllerPD
+    :public dg::Entity
+    {
+    public:
+      static const std::string CLASS_NAME;
+
+    public: /* --- CONSTRUCTION --- */
+
+      ControllerPD( const std::string& name );
+      virtual ~ControllerPD( 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 --- */
+      void fromSensor(const bool& inFromSensor) {    fromSensor_ = inFromSensor;  }
+      bool fromSensor() const {    return fromSensor_;  }
+    private:
+      bool fromSensor_;
+      virtual void commandLine( const std::string& cmdLine,
+				std::istringstream& cmdArgs,
+				std::ostream& os );
+    };
+
+
+
+  } // namespace dyninv
+} // namespace sot
+
+
+
+#endif // #ifndef __sot_dyninv_ControllerPD_H__

sot-dyninv.pc.cmake

+prefix=${CMAKE_INSTALL_PREFIX}
+exec_prefix=${install_pkg_prefix}
+libdir=${install_pkg_exec_prefix}/lib/plugin
+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}

src/CMakeLists.txt

+# Copyright 2010, François Bleibel, Olivier Stasse, JRL, CNRS/AIST,
+#                 Florent Lamiraux (CNRS/LAAS)
+#
+# This file is part of sot-dyninv.
+# sot-dyninv is free software: you can redistribute it and/or
+# modify it under the terms of the GNU Lesser General Public License
+# as published by the Free Software Foundation, either version 3 of
+# the License, or (at your option) any later version.
+#
+# sot-dyninv is distributed in the hope that it will be useful, but
+# WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+# General Lesser Public License for more details.  You should have
+# received a copy of the GNU Lesser General Public License along with
+# sot-dyninv. If not, see <http://www.gnu.org/licenses/>.
+
+INCLUDE(../cmake/python.cmake)
+
+IF(CMAKE_BUILD_TYPE STREQUAL "DEBUG")
+  ADD_DEFINITIONS(-DDEBUG=2)
+ENDIF(CMAKE_BUILD_TYPE STREQUAL "DEBUG")
+
+
+FOREACH(lib ${libs})
+  ADD_LIBRARY(${lib} SHARED ${lib}.cpp)
+
+  SET_TARGET_PROPERTIES(${lib} PROPERTIES
+    PREFIX ""
+    SOVERSION ${PROJECT_VERSION})
+
+  IF(${lib}_plugins_dependencies)
+    ADD_DEPENDENCIES(${lib} "${${lib}_plugins_dependencies}")
+    TARGET_LINK_LIBRARIES(${lib} "${${lib}_plugins_dependencies}")
+  ENDIF(${lib}_plugins_dependencies)
+
+  PKG_CONFIG_USE_DEPENDENCY(${lib} sot-core)
+  PKG_CONFIG_USE_DEPENDENCY(${lib} dynamic-graph)
+
+  INSTALL(TARGETS ${lib} DESTINATION lib/plugin)
+  # build python submodule
+  STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${lib})
+  ADD_DEPENDENCIES(${lib} MKDIR_${PYTHON_LIBRARY_NAME})
+  ADD_CUSTOM_TARGET(MKDIR_${PYTHON_LIBRARY_NAME}
+    mkdir -p dynamic_graph/sot/dyninv/${PYTHON_LIBRARY_NAME}
+    )
+  DYNAMIC_GRAPH_PYTHON_MODULE("sot/dyninv/${PYTHON_LIBRARY_NAME}"
+    ${lib}
+    sot/dyninv/${PYTHON_LIBRARY_NAME}/wrap
+    )
+ENDFOREACH(lib)
+
+EXEC_PROGRAM(${PYTHON_EXECUTABLE} ARGS "-c \"from distutils import sysconfig; print sysconfig.get_python_lib(0,0,prefix='')\""
+  OUTPUT_VARIABLE PYTHON_SITELIB
+  )
+
+# Install empty __init__.py files in intermediate directories.
+INSTALL(FILES
+  ${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dyninv/__init__.py
+  DESTINATION ${PYTHON_SITELIB}/dynamic_graph/sot/dyninv
+  )

src/controller-pd.cpp

+/*
+ * Copyright 2011, Nicolas Mansard, LAAS-CNRS
+ *
+ * This file is part of sot-dyninv.
+ * sot-dyninv is free software: you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public License
+ * as published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ * sot-dyninv is distributed in the hope that it will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License for more details.  You should
+ * have received a copy of the GNU Lesser General Public License along
+ * with sot-dyninv.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <sot-dyninv/controller-pd.h>
+#include <sot-core/debug.h>
+#include <dynamic-graph/factory.h>
+#include <dynamic-graph/command.h>
+#include <dynamic-graph/command-setter.h>
+#include <dynamic-graph/command-getter.h>
+
+#include <jrl/mal/matrixabstractlayer.hh>
+
+namespace sot
+{
+  namespace dyninv
+  {
+
+    using namespace dg;
+
+    DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ControllerPD,"ControllerPD");
+
+    ControllerPD::
+    ControllerPD( const std::string & name )
+      :Entity(name)
+      ,sensorWorldRotationSIN(NULL,"sotControllerPD("+name
+			      +")::input(MatrixRotation)::sensorWorldRotation")
+      ,sensorEmbeddedPositionSIN(NULL,"sotControllerPD("+name
+				 +")::input(MatrixHomo)::sensorEmbeddedPosition")
+      ,contactWorldPositionSIN(NULL,"sotControllerPD("+name
+			       +")::input(MatrixHomo)::contactWorldPosition")
+      ,contactEmbeddedPositionSIN(NULL,"sotControllerPD("+name
+				  +")::input(MatrixHomo)::contactEmbeddedPosition")
+
+      ,anglesSOUT( boost::bind(&ControllerPD::computeAngles,this,_1,_2),
+		   sensorWorldRotationSIN<<sensorEmbeddedPositionSIN
+		   <<contactWorldPositionSIN<<contactEmbeddedPositionSIN,
+		   "sotControllerPD("+name+")::output(Vector)::angles" )
+      ,flexibilitySOUT( boost::bind(&ControllerPD::computeFlexibilityFromAngles
+				    ,this,_1,_2),
+			anglesSOUT,
+			"sotControllerPD("+name+")::output(matrixRotation)::flexibility" )
+      ,driftSOUT( boost::bind(&ControllerPD::computeDriftFromAngles,this,_1,_2),
+		  anglesSOUT,
+		  "sotControllerPD("+name+")::output(matrixRotation)::drift" )
+      ,sensorWorldRotationSOUT( boost::bind(&ControllerPD::computeSensorWorldRotation
+					    ,this,_1,_2),
+				anglesSOUT<<sensorWorldRotationSIN,
+				"sotControllerPD("+name
+				+")::output(matrixRotation)::sensorCorrectedRotation" )
+      ,waistWorldRotationSOUT( boost::bind(&ControllerPD::computeWaistWorldRotation
+					   ,this,_1,_2),
+			       sensorWorldRotationSOUT<<sensorEmbeddedPositionSIN,
+			       "sotControllerPD("+name
+			       +")::output(matrixRotation)::waistWorldRotation" )
+      ,waistWorldPositionSOUT( boost::bind(&ControllerPD::computeWaistWorldPosition
+					   ,this,_1,_2),
+			       flexibilitySOUT << contactEmbeddedPositionSIN,
+			       "sotControllerPD("+name
+			       +")::output(MatrixHomogeneous)::waistWorldPosition" )
+      ,waistWorldPoseRPYSOUT( boost::bind(&ControllerPD::computeWaistWorldPoseRPY
+					  ,this,_1,_2),
+			      waistWorldPositionSOUT,
+			      "sotControllerPD("+name
+			      +")::output(vectorRollPitchYaw)::waistWorldPoseRPY" )
+
+      ,fromSensor_(true)
+    {
+      sotDEBUGIN(5);
+
+      signalRegistration( sensorWorldRotationSIN     << sensorEmbeddedPositionSIN
+			  << contactWorldPositionSIN << contactEmbeddedPositionSIN
+			  << anglesSOUT              << flexibilitySOUT
+			  << driftSOUT               << sensorWorldRotationSOUT
+			  << waistWorldRotationSOUT
+			  << waistWorldPositionSOUT  << waistWorldPoseRPYSOUT  );
+
+      // Commands
+      std::string docstring;
+      docstring = "    \n"
+	"    Set flag specifying whether angle is measured from sensors or simulated.\n"
+	"    \n"
+	"      Input:\n"
+	"        - a boolean value.\n"
+	"    \n";
+      addCommand("setFromSensor",
+		 new ::dynamicgraph::command::Setter<ControllerPD, bool>
+		 (*this, &ControllerPD::fromSensor, docstring));
+
+      docstring = "    \n"
+	"    Get flag specifying whether angle is measured from sensors or simulated.\n"
+	"    \n"
+	"      No input,\n"
+	"      return a boolean value.\n"
+	"    \n";
+      addCommand("getFromSensor",
+		 new ::dynamicgraph::command::Getter<ControllerPD, bool>
+		 (*this, &ControllerPD::fromSensor, docstring));
+
+      sotDEBUGOUT(5);
+    }
+
+
+    ControllerPD::
+    ~ControllerPD( void )
+    {
+      sotDEBUGIN(5);
+
+      sotDEBUGOUT(5);
+      return;
+    }
+
+    /* --- SIGNALS -------------------------------------------------------------- */
+    /* --- SIGNALS -------------------------------------------------------------- */
+    /* --- SIGNALS -------------------------------------------------------------- */
+    ml::Vector& ControllerPD::
+    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& ControllerPD::
+    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& ControllerPD::
+    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& ControllerPD::
+    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& ControllerPD::
+    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& ControllerPD::
+    computeWaistWorldPosition( MatrixHomogeneous& res,
+			       const int& time )
+    {
+      sotDEBUGIN(15);
+  
+      const MatrixHomogeneous & waistMleg = contactEmbeddedPositionSIN( time );
+      const MatrixHomogeneous& contactPos = contactWorldPositionSIN( time );
+      MatrixHomogeneous legMwaist; waistMleg.inverse( legMwaist );
+      MatrixHomogeneous tmpRes;
+      if( fromSensor_ )
+	{ 
+	  const MatrixRotation & Rflex = flexibilitySOUT( time ); // footRleg
+	  ml::Vector zero(3); zero.fill(0.);
+	  MatrixHomogeneous footMleg; footMleg.buildFrom( Rflex,zero );
+				    
+	  footMleg.multiply( legMwaist,tmpRes );
+	}
+      else { tmpRes = legMwaist; }
+
+      contactPos.multiply( tmpRes, res );
+      sotDEBUGOUT(15);
+      return res;
+    }
+
+    ml::Vector& ControllerPD::
+    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 ControllerPD::
+    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); }
+    }
+
+  } // namespace dyninv
+} // namespace sot
+

src/dynamic_graph/sot/dyninv/__init__.py

+from controller_pd import ControllerPD
+ControllerPD('')

unitTesting/CMakeLists.txt

+# Copyright 2011, François Bleibel, Olivier Stasse, JRL, CNRS/AIST
+#
+# This file is part of sot-dyninv.
+# sot-dyninv is free software: you can redistribute it and/or
+# modify it under the terms of the GNU Lesser General Public License
+# as published by the Free Software Foundation, either version 3 of
+# the License, or (at your option) any later version.
+#
+# sot-dyninv is distributed in the hope that it will be useful, but
+# WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+# General Lesser Public License for more details.  You should have
+# received a copy of the GNU Lesser General Public License along with
+# sot-dyninv. If not, see <http://www.gnu.org/licenses/>.
+
+ADD_DEFINITIONS(-DDEBUG=2)
+
+SET(tests
+  dummy)
+
+FOREACH(test ${tests})
+  SET(EXECUTABLE_NAME "${test}_exe")
+  ADD_EXECUTABLE(${EXECUTABLE_NAME}
+    ${test}.cpp)
+
+  TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME}
+    controller-pd
+    )
+
+  PKG_CONFIG_USE_DEPENDENCY(${EXECUTABLE_NAME} sot-core)
+
+  IF(${test}_plugins_dependencies)
+    ADD_DEPENDENCIES(${EXECUTABLE_NAME} "${${test}_plugins_dependencies}")
+    TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} "${${test}_plugins_dependencies}")
+  ENDIF(${test}_plugins_dependencies)
+
+
+  ADD_TEST(${test} ${EXECUTABLE_NAME}
+    ${samplemodelpath} sample.wrl ${samplespec} ${sampleljr} )
+
+  IF (UNIX)
+    SET_PROPERTY(TEST ${test} PROPERTY
+      ENVIRONMENT "LD_LIBRARY_PATH=${CMAKE_INSTALL_PREFIX}/lib:${CMAKE_BINARY_DIR}/src:${BOOST_ROOT}/lib")
+  ENDIF(UNIX)
+
+ENDFOREACH(test)

unitTesting/dummy.cpp

+/*
+ * Copyright 2011, Nicolas Mansard, LAAS-CNRS
+ *
+ * This file is part of sot-dyninv.
+ * sot-dyninv is free software: you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public License
+ * as published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ * sot-dyninv is distributed in the hope that it will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License for more details.  You should
+ * have received a copy of the GNU Lesser General Public License along
+ * with sot-dyninvc.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+int main (void)
+{
+
+}