diff --git a/doc/.svn/all-wcprops b/doc/.svn/all-wcprops
deleted file mode 100644
index 5cdf382a5fc73aa5c0e47b47f892c05cd3ab3583..0000000000000000000000000000000000000000
--- a/doc/.svn/all-wcprops
+++ /dev/null
@@ -1,23 +0,0 @@
-K 25
-svn:wc:ra_dav:version-url
-V 85
-/labrobotica/!svn/ver/9370/restricted/algorithms/quadarm_task_priority_ctrl/trunk/doc
-END
-main.dox
-K 25
-svn:wc:ra_dav:version-url
-V 94
-/labrobotica/!svn/ver/9370/restricted/algorithms/quadarm_task_priority_ctrl/trunk/doc/main.dox
-END
-doxygen.conf
-K 25
-svn:wc:ra_dav:version-url
-V 98
-/labrobotica/!svn/ver/9370/restricted/algorithms/quadarm_task_priority_ctrl/trunk/doc/doxygen.conf
-END
-doxygen_project_name.conf
-K 25
-svn:wc:ra_dav:version-url
-V 111
-/labrobotica/!svn/ver/9370/restricted/algorithms/quadarm_task_priority_ctrl/trunk/doc/doxygen_project_name.conf
-END
diff --git a/doc/.svn/entries b/doc/.svn/entries
deleted file mode 100644
index a8314393f47713942f0c763eb5af095390ce51e9..0000000000000000000000000000000000000000
--- a/doc/.svn/entries
+++ /dev/null
@@ -1,136 +0,0 @@
-10
-
-dir
-11923
-https://devel.iri.upc.edu/labrobotica/restricted/algorithms/quadarm_task_priority_ctrl/trunk/doc
-https://devel.iri.upc.edu/labrobotica
-
-
-
-2013-12-21T12:46:48.834587Z
-9109
-asantamaria
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-224674b8-e365-4e73-a4a8-558dbbfec58c
-
-images
-dir
-
-doxygen_project_name.conf
-file
-
-
-
-
-2014-10-10T14:10:57.500107Z
-b1add26bfbd2792446194a9355680a0f
-2013-07-26T11:11:23.878910Z
-8015
-asantamaria
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-44
-
-main.dox
-file
-
-
-
-
-2014-10-10T14:10:57.500107Z
-72cdffd3150db9c645cb09e5573f9ad8
-2013-12-21T12:46:48.834587Z
-9109
-asantamaria
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-3968
-
-doxygen.conf
-file
-
-
-
-
-2014-10-10T14:10:57.500107Z
-212ea203c326e25dd5ecfe4b853e0142
-2013-07-26T11:11:23.878910Z
-8015
-asantamaria
-has-props
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-9260
-
-files
-dir
-
diff --git a/doc/.svn/prop-base/doxygen.conf.svn-base b/doc/.svn/prop-base/doxygen.conf.svn-base
deleted file mode 100644
index 869ac71cf7e4d72d9ab52f86d630c1c3f0c017ce..0000000000000000000000000000000000000000
--- a/doc/.svn/prop-base/doxygen.conf.svn-base
+++ /dev/null
@@ -1,5 +0,0 @@
-K 14
-svn:executable
-V 1
-*
-END
diff --git a/doc/.svn/text-base/doxygen.conf.svn-base b/doc/.svn/text-base/doxygen.conf.svn-base
deleted file mode 100644
index 347af2ef86bf4786d49b13d48c12146b0d001b01..0000000000000000000000000000000000000000
--- a/doc/.svn/text-base/doxygen.conf.svn-base
+++ /dev/null
@@ -1,251 +0,0 @@
-# Doxyfile 1.5.5
-
-@INCLUDE_PATH = ../doc/
-@INCLUDE = doxygen_project_name.conf
-
-#---------------------------------------------------------------------------
-# Project related configuration options
-#---------------------------------------------------------------------------
-DOXYFILE_ENCODING = UTF-8
-PROJECT_NUMBER =
-OUTPUT_DIRECTORY = ../doc
-CREATE_SUBDIRS = NO
-OUTPUT_LANGUAGE = English
-BRIEF_MEMBER_DESC = YES
-REPEAT_BRIEF = NO
-ABBREVIATE_BRIEF =
-ALWAYS_DETAILED_SEC = NO
-INLINE_INHERITED_MEMB = NO
-FULL_PATH_NAMES = YES
-STRIP_FROM_PATH =
-STRIP_FROM_INC_PATH =
-SHORT_NAMES = NO
-JAVADOC_AUTOBRIEF = NO
-QT_AUTOBRIEF = NO
-MULTILINE_CPP_IS_BRIEF = NO
-DETAILS_AT_TOP = NO
-INHERIT_DOCS = YES
-SEPARATE_MEMBER_PAGES = NO
-TAB_SIZE = 8
-ALIASES =
-OPTIMIZE_OUTPUT_FOR_C = YES
-OPTIMIZE_OUTPUT_JAVA = NO
-OPTIMIZE_FOR_FORTRAN = NO
-OPTIMIZE_OUTPUT_VHDL = NO
-BUILTIN_STL_SUPPORT = NO
-CPP_CLI_SUPPORT = NO
-SIP_SUPPORT = NO
-DISTRIBUTE_GROUP_DOC = NO
-SUBGROUPING = YES
-TYPEDEF_HIDES_STRUCT = NO
-#---------------------------------------------------------------------------
-# Build related configuration options
-#---------------------------------------------------------------------------
-EXTRACT_ALL = NO
-EXTRACT_PRIVATE = NO
-EXTRACT_STATIC = NO
-EXTRACT_LOCAL_CLASSES = YES
-EXTRACT_LOCAL_METHODS = NO
-EXTRACT_ANON_NSPACES = NO
-HIDE_UNDOC_MEMBERS = NO
-HIDE_UNDOC_CLASSES = NO
-HIDE_FRIEND_COMPOUNDS = NO
-HIDE_IN_BODY_DOCS = NO
-INTERNAL_DOCS = NO
-CASE_SENSE_NAMES = YES
-HIDE_SCOPE_NAMES = NO
-SHOW_INCLUDE_FILES = YES
-INLINE_INFO = YES
-SORT_MEMBER_DOCS = NO
-SORT_BRIEF_DOCS = NO
-SORT_GROUP_NAMES = NO
-SORT_BY_SCOPE_NAME = NO
-GENERATE_TODOLIST = YES
-GENERATE_TESTLIST = YES
-GENERATE_BUGLIST = YES
-GENERATE_DEPRECATEDLIST= YES
-ENABLED_SECTIONS =
-MAX_INITIALIZER_LINES = 30
-SHOW_USED_FILES = YES
-SHOW_DIRECTORIES = YES
-FILE_VERSION_FILTER =
-#---------------------------------------------------------------------------
-# configuration options related to warning and progress messages
-#---------------------------------------------------------------------------
-QUIET = YES
-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
-#---------------------------------------------------------------------------
-INPUT = ../src \
- ../doc/main.dox
-INPUT_ENCODING = UTF-8
-FILE_PATTERNS = *.c \
- *.h \
- *.cpp
-RECURSIVE = YES
-EXCLUDE =
-EXCLUDE_SYMLINKS = NO
-EXCLUDE_PATTERNS = *.tab.c \
- *.tab.h \
- lex* \
- *glr.h \
- *llr.h \
- *glr.c \
- *llr.c \
- *general.h
-EXCLUDE_SYMBOLS =
-EXAMPLE_PATH = ../src/examples
-EXAMPLE_PATTERNS =
-EXAMPLE_RECURSIVE = NO
-IMAGE_PATH = ../doc/images
-INPUT_FILTER =
-FILTER_PATTERNS =
-FILTER_SOURCE_FILES = NO
-#---------------------------------------------------------------------------
-# configuration options related to source browsing
-#---------------------------------------------------------------------------
-SOURCE_BROWSER = YES
-INLINE_SOURCES = NO
-STRIP_CODE_COMMENTS = YES
-REFERENCED_BY_RELATION = YES
-REFERENCES_RELATION = YES
-REFERENCES_LINK_SOURCE = YES
-USE_HTAGS = NO
-VERBATIM_HEADERS = YES
-#---------------------------------------------------------------------------
-# configuration options related to the alphabetical class index
-#---------------------------------------------------------------------------
-ALPHABETICAL_INDEX = YES
-COLS_IN_ALPHA_INDEX = 5
-IGNORE_PREFIX =
-#---------------------------------------------------------------------------
-# configuration options related to the HTML output
-#---------------------------------------------------------------------------
-GENERATE_HTML = YES
-HTML_OUTPUT = html
-HTML_FILE_EXTENSION = .html
-HTML_HEADER =
-HTML_FOOTER =
-HTML_STYLESHEET =
-HTML_ALIGN_MEMBERS = YES
-GENERATE_HTMLHELP = NO
-GENERATE_DOCSET = NO
-DOCSET_FEEDNAME = "Doxygen generated docs"
-DOCSET_BUNDLE_ID = org.doxygen.Project
-HTML_DYNAMIC_SECTIONS = NO
-CHM_FILE =
-HHC_LOCATION =
-GENERATE_CHI = NO
-BINARY_TOC = NO
-TOC_EXPAND = NO
-DISABLE_INDEX = NO
-ENUM_VALUES_PER_LINE = 4
-GENERATE_TREEVIEW = NO
-TREEVIEW_WIDTH = 250
-#---------------------------------------------------------------------------
-# configuration options related to the LaTeX output
-#---------------------------------------------------------------------------
-GENERATE_LATEX = NO
-LATEX_OUTPUT = latex
-LATEX_CMD_NAME = latex
-MAKEINDEX_CMD_NAME = makeindex
-COMPACT_LATEX = NO
-PAPER_TYPE = a4
-EXTRA_PACKAGES =
-LATEX_HEADER =
-PDF_HYPERLINKS = YES
-USE_PDFLATEX = NO
-LATEX_BATCHMODE = NO
-LATEX_HIDE_INDICES = NO
-#---------------------------------------------------------------------------
-# configuration options related to the RTF output
-#---------------------------------------------------------------------------
-GENERATE_RTF = NO
-RTF_OUTPUT = rtf
-COMPACT_RTF = NO
-RTF_HYPERLINKS = NO
-RTF_STYLESHEET_FILE =
-RTF_EXTENSIONS_FILE =
-#---------------------------------------------------------------------------
-# configuration options related to the man page output
-#---------------------------------------------------------------------------
-GENERATE_MAN = NO
-MAN_OUTPUT = man
-MAN_EXTENSION = .3
-MAN_LINKS = NO
-#---------------------------------------------------------------------------
-# configuration options related to the XML output
-#---------------------------------------------------------------------------
-GENERATE_XML = NO
-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
-#---------------------------------------------------------------------------
-GENERATE_PERLMOD = NO
-PERLMOD_LATEX = NO
-PERLMOD_PRETTY = YES
-PERLMOD_MAKEVAR_PREFIX =
-#---------------------------------------------------------------------------
-# Configuration options related to the preprocessor
-#---------------------------------------------------------------------------
-ENABLE_PREPROCESSING = YES
-MACRO_EXPANSION = NO
-EXPAND_ONLY_PREDEF = NO
-SEARCH_INCLUDES = YES
-INCLUDE_PATH =
-INCLUDE_FILE_PATTERNS =
-PREDEFINED = _USE_MPI=1
-EXPAND_AS_DEFINED =
-SKIP_FUNCTION_MACROS = YES
-#---------------------------------------------------------------------------
-# Configuration::additions related to external references
-#---------------------------------------------------------------------------
-TAGFILES =
-GENERATE_TAGFILE =
-ALLEXTERNALS = NO
-EXTERNAL_GROUPS = YES
-PERL_PATH = /usr/bin/perl
-#---------------------------------------------------------------------------
-# Configuration options related to the dot tool
-#---------------------------------------------------------------------------
-CLASS_DIAGRAMS = YES
-MSCGEN_PATH =
-HIDE_UNDOC_RELATIONS = YES
-HAVE_DOT = YES
-CLASS_GRAPH = YES
-COLLABORATION_GRAPH = YES
-GROUP_GRAPHS = YES
-UML_LOOK = NO
-TEMPLATE_RELATIONS = NO
-INCLUDE_GRAPH = NO
-INCLUDED_BY_GRAPH = NO
-CALL_GRAPH = YES
-CALLER_GRAPH = YES
-GRAPHICAL_HIERARCHY = YES
-DIRECTORY_GRAPH = NO
-DOT_IMAGE_FORMAT = png
-DOT_PATH =
-DOTFILE_DIRS =
-DOT_GRAPH_MAX_NODES = 50
-MAX_DOT_GRAPH_DEPTH = 2
-DOT_TRANSPARENT = YES
-DOT_MULTI_TARGETS = NO
-GENERATE_LEGEND = YES
-DOT_CLEANUP = YES
-#---------------------------------------------------------------------------
-# Configuration::additions related to the search engine
-#---------------------------------------------------------------------------
-SEARCHENGINE = NO
diff --git a/doc/.svn/text-base/doxygen_project_name.conf.svn-base b/doc/.svn/text-base/doxygen_project_name.conf.svn-base
deleted file mode 100644
index 941d791ab1199b066e9b5aa670df114de6989ec5..0000000000000000000000000000000000000000
--- a/doc/.svn/text-base/doxygen_project_name.conf.svn-base
+++ /dev/null
@@ -1 +0,0 @@
-PROJECT_NAME = "quadarm_task_priority_ctrl"
diff --git a/doc/.svn/text-base/main.dox.svn-base b/doc/.svn/text-base/main.dox.svn-base
deleted file mode 100644
index e3bfbc9eba01f7c8b3b766b2e1dae088785f9ae6..0000000000000000000000000000000000000000
--- a/doc/.svn/text-base/main.dox.svn-base
+++ /dev/null
@@ -1,120 +0,0 @@
-/*! \mainpage quadarm_task_priority_ctrl
-
- \section Introduction
-
- \subsection Pre-Requisites
-
- This package requires of the following libraries and packages
- - <A href="http://www.cmake.org">cmake</A>, a cross-platform build system.
- - <A href="http://www.doxygen.org">doxygen</a> and
- <A href="http://www.graphviz.org">graphviz</a> to generate the documentation.
- - stdc++,
- - <A href="http://www.orocos.org/kdl">Orocos KDL</a> to solve arm kinematics.
- .
-
- Orocos KDL installation:
-
- >> git clone http://git.mech.kuleuven.be/robotics/orocos_kinematics_dynamics.git
- >> cd orocos_kinematics_dynamics/orocos_kdl
- >> mkdir build; cd build
- >> cmake ..
-
- Then press [c], [e] and [g].
-
- >> sudo make install
-
-
- Under linux all of these utilities are available in ready-to-use packages.
-
- Under MacOS most of the packages are available via <a href="http://www.finkproject.org/">fink</a>. <br>
-
- \subsection Compilation
-
- Just download this package, uncompress it, and execute
- - cd build
- - cmake ..
- .
- to generate the makefile and then
- - make
- .
- to obtain the shared library (in this case called <em>iriutils.so</em>) and
- also all the example programs.
-
- The <em>cmake</em> only need to be executed once (make will automatically call
- <em>cmake</em> if you modify one of the <em>CMakeList.txt</em> files).
-
- To generate this documentation type
- - make doc
- .
-
- The files in the <em>build</em> directory are genetated by <em>cmake</em>
- and <em>make</em> and can be safely removed.
- After doing so you will need to call cmake manually again.
-
- \subsection Configuration
-
- The default build mode is DEBUG. That is, objects and executables
- include debug information.
-
- The RELEASE build mode optimizes for speed. To build in this mode
- execute
- - cmake .. -DCMAKE_BUILD_TYPE=RELEASE
- .
- The release mode will be kept until next time cmake is executed.
-
- \subsection Installation
-
- In order to be able to use the library, it it necessary to copy it into the system.
- To do that, execute
-
- - make install
- .
-
- as root and the shared libraries will be copied to <em>/usr/local/lib/iriutils</em> directory
- and the header files will be copied to <em>/usr/local/include/iriutils</em> dierctory. At
- this point, the library may be used by any user.
-
- To remove the library from the system, exceute
- - make uninstall
- .
- as root, and all the associated files will be removed from the system.
-
- \section Customization
-
- To build a new application using these library, first it is necessary to locate if the library
- has been installed or not using the following command
-
- - FIND_PACKAGE(quadarm_task_priority_ctrl REQUIRED)
-
- In the case that the package is present, it is necessary to add the header files directory to
- the include directory path by using
-
- - INCLUDE_DIRECTORIES(${quadarm_task_priority_ctrl_INCLUDE_DIR})
-
- Finally, it is also nevessary to link with the desired libraries by using the following command
-
- - TARGET_LINK_LIBRARIES(<executable name> ${quadarm_task_priority_ctrl_LIBRARY})
- .
-
- \section License
-
- This package is licensed under a
- <a href="http://www.gnu.org/licenses/lgpl.html">
- LGPL 3.0 License</a>.
-
- \section Disclaimer
-
- This 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.
-
- This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
-
- */
diff --git a/doc/files/.svn/all-wcprops b/doc/files/.svn/all-wcprops
deleted file mode 100644
index 2125cf3fc7a5b16b071965d763ae8372a3c8a35d..0000000000000000000000000000000000000000
--- a/doc/files/.svn/all-wcprops
+++ /dev/null
@@ -1,5 +0,0 @@
-K 25
-svn:wc:ra_dav:version-url
-V 91
-/labrobotica/!svn/ver/9370/restricted/algorithms/quadarm_task_priority_ctrl/trunk/doc/files
-END
diff --git a/doc/files/.svn/entries b/doc/files/.svn/entries
deleted file mode 100644
index fa5cd0d1d354706cf78e2f818d16ba6fe5d7e8d5..0000000000000000000000000000000000000000
--- a/doc/files/.svn/entries
+++ /dev/null
@@ -1,28 +0,0 @@
-10
-
-dir
-11923
-https://devel.iri.upc.edu/labrobotica/restricted/algorithms/quadarm_task_priority_ctrl/trunk/doc/files
-https://devel.iri.upc.edu/labrobotica
-
-
-
-2013-07-26T11:11:23.878910Z
-8015
-asantamaria
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-224674b8-e365-4e73-a4a8-558dbbfec58c
-
diff --git a/doc/images/.svn/all-wcprops b/doc/images/.svn/all-wcprops
deleted file mode 100644
index 23c61caf24433e6e69299a3507fa2b9b1f4a1439..0000000000000000000000000000000000000000
--- a/doc/images/.svn/all-wcprops
+++ /dev/null
@@ -1,5 +0,0 @@
-K 25
-svn:wc:ra_dav:version-url
-V 92
-/labrobotica/!svn/ver/9370/restricted/algorithms/quadarm_task_priority_ctrl/trunk/doc/images
-END
diff --git a/doc/images/.svn/entries b/doc/images/.svn/entries
deleted file mode 100644
index 762d3b3a624724d0776212042ba9d5fa4fa5447a..0000000000000000000000000000000000000000
--- a/doc/images/.svn/entries
+++ /dev/null
@@ -1,28 +0,0 @@
-10
-
-dir
-11923
-https://devel.iri.upc.edu/labrobotica/restricted/algorithms/quadarm_task_priority_ctrl/trunk/doc/images
-https://devel.iri.upc.edu/labrobotica
-
-
-
-2013-07-26T11:11:23.878910Z
-8015
-asantamaria
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-224674b8-e365-4e73-a4a8-558dbbfec58c
-
diff --git a/src/.svn/all-wcprops b/src/.svn/all-wcprops
deleted file mode 100644
index 3fec2ec80a32af74c9af6544f0cc2c4a58f2b19d..0000000000000000000000000000000000000000
--- a/src/.svn/all-wcprops
+++ /dev/null
@@ -1,23 +0,0 @@
-K 25
-svn:wc:ra_dav:version-url
-V 86
-/labrobotica/!svn/ver/11061/restricted/algorithms/quadarm_task_priority_ctrl/trunk/src
-END
-quadarm_task_priority_ctrl.h
-K 25
-svn:wc:ra_dav:version-url
-V 115
-/labrobotica/!svn/ver/11045/restricted/algorithms/quadarm_task_priority_ctrl/trunk/src/quadarm_task_priority_ctrl.h
-END
-CMakeLists.txt
-K 25
-svn:wc:ra_dav:version-url
-V 100
-/labrobotica/!svn/ver/9370/restricted/algorithms/quadarm_task_priority_ctrl/trunk/src/CMakeLists.txt
-END
-quadarm_task_priority_ctrl.cpp
-K 25
-svn:wc:ra_dav:version-url
-V 117
-/labrobotica/!svn/ver/11061/restricted/algorithms/quadarm_task_priority_ctrl/trunk/src/quadarm_task_priority_ctrl.cpp
-END
diff --git a/src/.svn/entries b/src/.svn/entries
deleted file mode 100644
index e697bd14773a778cf2f014248b8a32bf7e92e702..0000000000000000000000000000000000000000
--- a/src/.svn/entries
+++ /dev/null
@@ -1,130 +0,0 @@
-10
-
-dir
-11923
-https://devel.iri.upc.edu/labrobotica/restricted/algorithms/quadarm_task_priority_ctrl/trunk/src
-https://devel.iri.upc.edu/labrobotica
-
-
-
-2014-06-28T14:21:17.117145Z
-11061
-asantamaria
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-224674b8-e365-4e73-a4a8-558dbbfec58c
-
-quadarm_task_priority_ctrl.cpp
-file
-
-
-
-
-2014-10-10T14:10:57.608107Z
-bae9cd504c2d5016ec57246da45d820b
-2014-06-28T14:21:17.117145Z
-11061
-asantamaria
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-28421
-
-quadarm_task_priority_ctrl.h
-file
-
-
-
-
-2014-10-10T14:10:57.608107Z
-9f949f117fc061ba2232729a6384a10a
-2014-06-26T17:22:07.031216Z
-11045
-asantamaria
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-8317
-
-CMakeLists.txt
-file
-
-
-
-
-2014-10-10T14:10:57.608107Z
-ca5102b29a3a5d71d960dda000c12396
-2013-07-26T11:11:23.878910Z
-8015
-asantamaria
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-1220
-
diff --git a/src/.svn/text-base/CMakeLists.txt.svn-base b/src/.svn/text-base/CMakeLists.txt.svn-base
deleted file mode 100644
index 70202a20d90414f8801b6771ad4347b9ff0013a2..0000000000000000000000000000000000000000
--- a/src/.svn/text-base/CMakeLists.txt.svn-base
+++ /dev/null
@@ -1,43 +0,0 @@
-# driver source files
-SET(sources quadarm_task_priority_ctrl.cpp)
-
-# application header files
-SET(headers quadarm_task_priority_ctrl.h)
-
-# locate the necessary dependencies
-
-# KDL #########
-INCLUDE (${PROJECT_SOURCE_DIR}/FindPkgConfig.cmake)
-
-SET( KDL_INSTALL ${CMAKE_INSTALL_PREFIX} CACHE PATH "The KDL installation directory.")
-INCLUDE (${PROJECT_SOURCE_DIR}/FindKDL.cmake)
-
-# Boost #######
-set(Boost_USE_STATIC_LIBS ON)
-set(Boost_USE_MULTITHREADED OFF)
-set(Boost_USE_STATIC_RUNTIME OFF)
-FIND_PACKAGE(Boost REQUIRED)
-
-# Eigen #######
-FIND_PACKAGE(Eigen REQUIRED)
-
-# add the necessary include directories
-INCLUDE_DIRECTORIES(. ${EIGEN_INCLUDE_DIR} ${Boost_INCLUDE_DIRS})
-
-# create the shared library
-ADD_LIBRARY(quadarm_task_priority_ctrl SHARED ${sources})
-
-TARGET_LINK_LIBRARIES(quadarm_task_priority_ctrl ${EIGEN_LIBRARY} ${Boost_LIBRARIES})
-
-
-# link necessary libraries
-INSTALL(TARGETS quadarm_task_priority_ctrl
- RUNTIME DESTINATION bin
- LIBRARY DESTINATION lib/iridrivers
- ARCHIVE DESTINATION lib/iridrivers)
-
-INSTALL(FILES ${headers} DESTINATION include/iridrivers)
-
-INSTALL(FILES ../Findquadarm_task_priority_ctrl.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
-
-#ADD_SUBDIRECTORY(examples)
diff --git a/src/.svn/text-base/quadarm_task_priority_ctrl.cpp.svn-base b/src/.svn/text-base/quadarm_task_priority_ctrl.cpp.svn-base
deleted file mode 100644
index 70c751cdb710bf4ee9e0925170e6bc5803ad489a..0000000000000000000000000000000000000000
--- a/src/.svn/text-base/quadarm_task_priority_ctrl.cpp.svn-base
+++ /dev/null
@@ -1,876 +0,0 @@
-#include "quadarm_task_priority_ctrl.h"
-
-using namespace Eigen;
-using namespace KDL;
-using namespace std;
-
-CQuadarm_Task_Priority_Ctrl::CQuadarm_Task_Priority_Ctrl()
-{
- // time (&this->time_ini_);
-}
-
-CQuadarm_Task_Priority_Ctrl::~CQuadarm_Task_Priority_Ctrl()
-{
-}
-
-void CQuadarm_Task_Priority_Ctrl::quadarm_task_priority_ctrl(const MatrixXd& cam_vel,
- const Matrix4d& Tcam_in_tip,
- const Matrix4d& Ttag_in_cam,
- const double& target_dist,
- const Chain& kdl_chain,
- const vector<arm_joint> joint_info,
- ctrl_params& ctrl_params,
- MatrixXd& joint_pos,
- MatrixXd& robot_vels)
-{
- this->vel_.cam = cam_vel;
- this->T_.cam_in_tip = Tcam_in_tip;
- this->T_.tag_in_cam = Ttag_in_cam;
- this->arm_.chain = kdl_chain;
- this->arm_.joint_info=joint_info;
- this->arm_.jnt_pos = joint_pos;
- this->target_dist_ = target_dist;
- this->ctrl_params_ = ctrl_params;
-
- // Arm DOFs
- this->arm_.nj = this->arm_.chain.getNrOfJoints();
-
- qa_kinematics();
- qa_control();
-
- // Arm positions increment
- this->arm_.jnt_pos.block(6,0,this->arm_.nj,1) = this->arm_.jnt_pos.block(6,0,this->arm_.nj,1)+(this->vel_.quadarm.block(6,0,this->arm_.nj,1) * this->ctrl_params_.dt);
- // Quadrotor positions increment
- this->arm_.jnt_pos.block(0,0,6,1) = this->arm_.jnt_pos.block(0,0,6,1)+(this->vel_.quadarm.block(0,0,6,1) * this->ctrl_params_.dt);
- // return values
- ctrl_params = this->ctrl_params_;
- joint_pos = this->arm_.jnt_pos;
- robot_vels = this->vel_.quadarm;
-}
-
-void CQuadarm_Task_Priority_Ctrl::qa_kinematics(){
-
- // Get arm Homogenous transform, arm tip in base_link
- this->T_.tip_in_baselink = arm_f_kinematics();
-
- // Update Transforms
- this->T_.cam_in_baselink = this->T_.tip_in_baselink * this->T_.cam_in_tip;
- this->T_.tag_in_baselink = this->T_.cam_in_baselink * this->T_.tag_in_cam;
-
- // Arm Jacobian
- MatrixXd Ja = arm_jacobian();
-
- // Quadrotor Jacobian body to inertial frames
- MatrixXd Jb2i = quadrotor_jacobian();
- // Inertial to body frames
- MatrixXd Ji2b = calcPinv(Jb2i);
-
- // Jacobian from quadrotor body to camera
- Matrix3d skew = Matrix3d::Zero();
- skew(0,1)=this->T_.cam_in_baselink(2,3);
- skew(0,2)=-this->T_.cam_in_baselink(1,3);
- skew(1,0)=-this->T_.cam_in_baselink(2,3);
- skew(1,2)=this->T_.cam_in_baselink(0,3);
- skew(2,0)=this->T_.cam_in_baselink(1,3);
- skew(2,1)=-this->T_.cam_in_baselink(0,3);
- MatrixXd Jb2c = MatrixXd::Zero(6,6);
- Jb2c.block(0,0,3,3) = Matrix3d::Identity();
- Jb2c.block(0,3,3,3) = skew;
- Jb2c.block(3,3,3,3) = Matrix3d::Identity();
-
- // Rotation of camera in body frame
- Matrix3d Rc_in_b = this->T_.cam_in_baselink.block(0,0,3,3);
- // Rotation of body in camera frame
- Matrix3d Rb_in_c = Rc_in_b.transpose();
-
- // Transform Jacobian from body to camera frame
- MatrixXd Rot = MatrixXd::Zero(6,6);
- Rot.block(0,0,3,3)=Rb_in_c;
- Rot.block(3,3,3,3)=Rb_in_c;
-
- // Whole robot Jacobian
- this->Jqa_ = MatrixXd::Zero(6,6+this->arm_.nj);
-
- this->Jqa_.block(0,0,6,6) = Rot*Jb2c*Ji2b;
- this->Jqa_.block(0,6,6,this->arm_.nj)=Rot*Ja;
-}
-
-MatrixXd CQuadarm_Task_Priority_Ctrl::quadrotor_jacobian()
-{
- double psi,theta,phi;
-
- phi = this->arm_.jnt_pos(3,0);
- theta = this->arm_.jnt_pos(4,0);
- // psi = this->arm_.jnt_pos(5,0);
- psi = 0;
-
- MatrixXd Jq(6,6);
- Jq(0,0) = cos(psi)*cos(theta);
- Jq(0,1) = sin(phi)*cos(psi)*sin(theta)-cos(phi)*sin(psi);
- Jq(0,2) = cos(phi)*cos(psi)*sin(theta)+sin(phi)*sin(psi);
- Jq(1,0) = sin(psi)*cos(theta);
- Jq(1,1) = sin(phi)*sin(psi)*sin(theta)+cos(phi)*cos(psi);
-
- Jq(1,2) = cos(phi)*sin(psi)*sin(theta)-sin(phi)*cos(psi);
- Jq(2,0) = -sin(theta);
- Jq(2,1) = sin(phi)*cos(theta);
- Jq(2,2) = cos(phi)*cos(theta);
- Jq.block(0,3,3,3) = MatrixXd::Zero(3,3);
- Jq.block(3,0,3,3) = MatrixXd::Zero(3,3);
- Jq(3,3) = 1;
- Jq(3,4) = sin(phi)*tan(theta);
- Jq(3,5) = cos(phi)*tan(theta);
- Jq(4,3) = 0;
- Jq(4,4) = cos(phi);
- Jq(4,5) = -sin(phi);
- Jq(5,3) = 0;
- Jq(5,4) = sin(phi)*cos(theta);
- Jq(5,5) = cos(phi)*cos(theta);
-
- return Jq;
-}
-
-MatrixXd CQuadarm_Task_Priority_Ctrl::arm_jacobian()
-{
- // constructs the kdl solver in non-realtime
- this->arm_.jnt_to_jac_solver.reset(new KDL::ChainJntToJacSolver(this->arm_.chain));
-
- // resize the joint state vector in non-realtime
- this->arm_.jacobian.resize(this->arm_.nj);
-
- // compute Jacobian in realtime
- this->arm_.jnt_to_jac_solver->JntToJac(this->arm_.jnt_pos_kdl, this->arm_.jacobian);
-
- MatrixXd Ja = MatrixXd::Zero(6,this->arm_.nj);
-
- for (int ii=0; ii<6; ++ii){
- for (unsigned int jj=0; jj<this->arm_.nj; ++jj){
- Ja(ii,jj) = this->arm_.jacobian.data(ii,jj);
- }
- }
- return Ja;
-}
-
-Matrix4d CQuadarm_Task_Priority_Ctrl::arm_f_kinematics()
-{
-
- // constructs the kdl solver in non-realtime
- this->arm_.jnt_to_pose_solver.reset(new KDL::ChainFkSolverPos_recursive(this->arm_.chain));
-
- // Create joint array
- this->arm_.jnt_pos_kdl = JntArray(this->arm_.nj);
-
- // resize the joint state vector in non-realtime
- this->arm_.jnt_pos_kdl.resize(this->arm_.nj);
-
- for(unsigned int i=0; i < this->arm_.nj; i++){
- this->arm_.jnt_pos_kdl.data(i) = this->arm_.jnt_pos(6+i,0);
- }
-
- // computes Cartesian pose in realtime From Base_link to Arm tip
- Frame kdlTarm;
- this->arm_.jnt_to_pose_solver->JntToCart(this->arm_.jnt_pos_kdl, kdlTarm);
-
- double roll,pitch,yaw;
- kdlTarm.M.GetEulerZYX(yaw,pitch,roll);
-
- Matrix4d Tarm;
- Matrix3d Rarm;
- // euler convention zyx
- Rarm = AngleAxisd(yaw, Vector3d::UnitZ()) \
- * AngleAxisd(pitch, Vector3d::UnitY()) \
- * AngleAxisd(roll, Vector3d::UnitX());
-
- Tarm.block(0,0,3,3) = Rarm;
- Tarm(0,3) = (double)kdlTarm.p.data[0];
- Tarm(1,3) = (double)kdlTarm.p.data[1];
- Tarm(2,3) = (double)kdlTarm.p.data[2];
- Tarm.row(3) << 0, 0, 0, 1;
-
- return Tarm;
-}
-
-void CQuadarm_Task_Priority_Ctrl::qa_control(){
-
- // Underactuated Quadrotor
- MatrixXd J1(8,4),J2(8,2),V1(4,1),Jqa1(6,4+this->arm_.nj),Jqa2(6,2),Vtotal(4+this->arm_.nj,1);
-
- // 9 DOF Extracting wx and wy from quadrotor //////////////////
- Jqa1.block(0,0,6,3) = this->Jqa_.block(0,0,6,3);
- Jqa1.block(0,3,6,1+this->arm_.nj) = this->Jqa_.block(0,5,6,1+this->arm_.nj);
- Jqa2 = this->Jqa_.block(0,3,6,2);
- MatrixXd eye(4+this->arm_.nj,4+this->arm_.nj);
- eye = MatrixXd::Identity(4+this->arm_.nj,4+this->arm_.nj);
-
- // compute pseudo inverse /////////////
- MatrixXd Jqa1_pseudo(4+this->arm_.nj,6);
- Jqa1_pseudo = calcPinv(Jqa1);
-
- MatrixXd Jqa1_geninverse(4+this->arm_.nj,6);
- weight_inverse(Jqa1,Jqa1_geninverse);
-
- // Control law ________________________________________________________________
-
- // Primary control task (Visual Servo) ________________________
- //this->ctrl_params_.vs_vel = Jqa1_pseudo * (this->vel_.cam-Jqa2*this->ctrl_params_.v_rollpitch);
- this->ctrl_params_.vs_vel = Jqa1_geninverse * (this->vel_.cam-Jqa2*this->ctrl_params_.v_rollpitch);
-
- // Secondary control task (CoG alignement) ____________________
-
- // Null space projector
- MatrixXd Nqa1 = (eye-(Jqa1_pseudo*Jqa1));
- //MatrixXd Nqa1 = (eye-(Jqa1_geninverse*Jqa1));
-
- // Secondary task Jacobian and sigma
- MatrixXd JG,JG_pseudo,sigmaG;
- sec_task_cog(JG,sigmaG);
- JG_pseudo = calcPinv(JG);
-
- // Gain
- double lambdaG = this->ctrl_params_.cog_gain;
-
- // Secondary task velocity
- this->ctrl_params_.cog_vel = Nqa1 * JG_pseudo * lambdaG * sigmaG;
-
- // Third control task (Joint limits) ________________________
-
- // Augmented null space projector from Augmented Jacobian
- MatrixXd Jqa1_G = MatrixXd::Zero(Jqa1.rows()+JG.rows(),Jqa1.cols());
- Jqa1_G.block(0,0,Jqa1.rows(),Jqa1.cols()) = Jqa1;
- Jqa1_G.block(Jqa1.rows()-1,0,JG.rows(),JG.cols())=JG;
- MatrixXd Nqa1_G = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj);
- MatrixXd Jqa1_G_pseudo = calcPinv(Jqa1_G);
- Nqa1_G = (eye-(Jqa1_G_pseudo*Jqa1_G));
-
- // Sec task Jacobian and sigma
- MatrixXd JL,JL_pseudo,sigmaL;
- sec_task_jl(JL,sigmaL);
- JL_pseudo = calcPinv(JL);
-
- // Gain
- double lambdaL = this->ctrl_params_.jntlim_gain;
-
- // Third task velocity
- this->ctrl_params_.jntlim_vel = Nqa1_G * JL_pseudo * lambdaL * sigmaL;
-
- //******************************************************************
-
- // this->ctrl_params_.cog_vel = Nqa1 * (JG_pseudo * lambdaG * sigmaG + JL_pseudo * lambdaL * sigmaL);
- // this->ctrl_params_.jntlim_vel = MatrixXd::Zero(9,1);
-
- //******************************************************************
- //Total velocity __________________________________________
- if (!this->ctrl_params_.enable_sec_task) {
- this->ctrl_params_.cog_vel = MatrixXd::Zero(4+this->arm_.nj,1);
- this->ctrl_params_.jntlim_vel = MatrixXd::Zero(4+this->arm_.nj,1);
- cout << "[Task Priority Control]: Secondary tasks not enabled" << endl;
- }
- Vtotal = this->ctrl_params_.vs_vel + this->ctrl_params_.cog_vel + this->ctrl_params_.jntlim_vel;
- Vtotal = this->ctrl_params_.lambda_robot.array()*Vtotal.array();
-
- // Check a singular configuration /////
- if (isnan(Vtotal(0,0)))
- {
- Vtotal=MatrixXd::Zero(4+this->arm_.nj,1);
- }
-
- // Rearranging terms
- this->vel_.quadarm = MatrixXd::Zero(6+this->arm_.nj,1);
- this->vel_.quadarm.block(0,0,3,1) = Vtotal.block(0,0,3,1);
- this->vel_.quadarm.block(5,0,1+this->arm_.nj,1) = Vtotal.block(3,0,1+this->arm_.nj,1);
-}
-
-void CQuadarm_Task_Priority_Ctrl::weight_inverse(const MatrixXd& J, MatrixXd& J_inverse)
-{
-
- double w_min = this->ctrl_params_.vs_delta_gain(0,0);
- double w_max = this->ctrl_params_.vs_delta_gain(1,0);
- double alpha_min = this->ctrl_params_.vs_alpha_min;
-
- double tmp = 2*3.14159*((this->target_dist_-w_min)/(w_max-w_min))-3.14159;
-
- double alpha = (0.5*(1+alpha_min)) + (0.5*(1-alpha_min))*tanh(tmp);
- // double alpha = (0.5*alpha_min) + (0.5*alpha_min*tanh(tmp));
-
- MatrixXd W = MatrixXd::Zero(4+this->arm_.nj,4+this->arm_.nj);
-
- for (unsigned int ii = 0; ii < 4+this->arm_.nj; ++ii)
- {
- // Quadrotor values
- if(ii<4)
- W(ii,ii) = 1-alpha;
- // Arm values
- else
- W(ii,ii) = alpha;
- }
-
- MatrixXd temp = J*W.inverse()*J.transpose();
-
- J_inverse = W.inverse()*J.transpose()*temp.inverse();
-}
-
-void CQuadarm_Task_Priority_Ctrl::sec_task_cog(MatrixXd& JG,MatrixXd& sigmaG)
-{
- // Initializations
- MatrixXd qa; // joint positions
- this->arm_.mass = 0; // Total arm mass
- MatrixXd mass = MatrixXd::Zero(this->arm_.nj,1); // Mass of the links
- this->ctrl_params_.cog_arm = MatrixXd::Zero(3,1); // Arm CoG
- MatrixXd arm_cg = MatrixXd::Zero(4,1); // Sum of CoG vector
- vector<Frame> joint_pose(this->arm_.nj); // Vector of Joint frames
- joint_pose.reserve(this->arm_.nj);
- vector<Matrix4d> Transf(this->arm_.nj); // Vector of HT of frames relative to each links
- Transf.reserve(this->arm_.nj);
- vector<MatrixXd> T_base_to_joint(this->arm_.nj+1); // Vector of HT of frame links relative to base
- T_base_to_joint.reserve(this->arm_.nj+1);
- vector<MatrixXd> IIIrdcolRot_b_x(this->arm_.nj+1); // Vector of r3d column of HT
- IIIrdcolRot_b_x.reserve(this->arm_.nj+1);
- vector<MatrixXd> frame_link(this->arm_.nj+1); // Origin of each link's frame
- frame_link.reserve(this->arm_.nj+1);
-
- vector<MatrixXd> dist(this->arm_.nj); // Vector of relative distance of each link to its CoGlink
- dist.reserve(this->arm_.nj);
- vector<MatrixXd> link_cg(this->arm_.nj); // Vector of link CoG relative to base
- link_cg.reserve(this->arm_.nj);
-
- // Get arm segments
- unsigned int js = this->arm_.chain.getNrOfSegments();
- vector<Segment> arm_segment(js);
- arm_segment.reserve(js);
- for (unsigned int ii = 1; ii < js; ++ii)
- {
- arm_segment.at(ii) = this->arm_.chain.getSegment(ii);
- }
-
- // Get joint positions
- qa = this->arm_.jnt_pos.block(6,0,this->arm_.nj,1);
-
- //Fixed transform from arm_base to base_link
- Matrix4d Tbase = Matrix4d::Zero();
- Tbase(3,3) = 1;
- Tbase(0,2) = -1;
- Tbase(1,1) = 1;
- Tbase(2,0) = 1;
-
- // Origin of the base frame
- MatrixXd pini(4,1);
- pini.col(0) << 0,0,0,1;
-
- // Initial transform
- T_base_to_joint.at(0) = Tbase;
- // 3rd column of rotation matrix
- IIIrdcolRot_b_x.at(0) = T_base_to_joint.at(0).block(0,2,3,1);
- // Origin of the initial frame
- frame_link.at(0) = T_base_to_joint.at(0) * pini;
-
- for (unsigned int jj = 0; jj < this->arm_.nj; ++jj)
- {
-
- // Get mass of each link
- mass(jj,0) = arm_segment.at(jj+1).getInertia().getMass();
- // Joint frames
- joint_pose.at(jj) = arm_segment.at(jj+1).pose(qa(jj,0));
- // relative Homogenous transforms (HT)
- Transf.at(jj) = GetTransform(joint_pose.at(jj));
- // HT r.t. base link
- T_base_to_joint.at(jj+1) = T_base_to_joint.at(jj) * Transf.at(jj);
- // 3rd column of rotation matrix
- IIIrdcolRot_b_x.at(jj+1) = T_base_to_joint.at(jj+1).block(0,2,3,1);
- // Origin of the link's frame r.t. base_link
- frame_link.at(jj+1) = T_base_to_joint.at(jj+1) * pini;
- // Distance to the middle of the link jj
- dist.at(jj) = (frame_link.at(jj+1)-frame_link.at(jj))/2;
- // gravity for the link jj
- link_cg.at(jj) = mass(jj,0)*(frame_link.at(jj)+dist.at(jj));
- // Sum of link CoGs
- arm_cg = arm_cg + link_cg.at(jj);
- // Sum of link mass
- this->arm_.mass = this->arm_.mass + mass(jj,0);
- }
-
- // vector from arm base to CoG
- this->ctrl_params_.cog_arm = arm_cg/this->arm_.mass;
-
- // Rotation between quadrotor body and inertial frames
- Matrix3d Rib;
- // Rib = AngleAxisd(0, Vector3d::UnitZ()) * AngleAxisd(this->ctrl_params_.q_rollpitch(1,0), Vector3d::UnitY()) * AngleAxisd(this->ctrl_params_.q_rollpitch(0,0), Vector3d::UnitX());
- Rib = MatrixXd::Identity(3,3);
-
- // X and Y values of CoG in quadrotor inertial frame
- MatrixXd cog_arm_in_qi = Rib * this->ctrl_params_.cog_arm.block(0,0,3,1);
- this->ctrl_params_.cog_PGxy = cog_arm_in_qi.block(0,0,2,1);
- MatrixXd X_partial;
-
- // Partial jacobian of each link (augmented links)
- MatrixXd Jj_cog(3,this->arm_.nj);
- for (unsigned int jj = 0; jj < this->arm_.nj; ++jj)
- {
- double partial_mass = 0;
- MatrixXd partial_arm_cg = MatrixXd::Zero(4,1);
-
- for (unsigned int ii = jj; ii < this->arm_.nj; ++ii)
- {
- partial_mass = partial_mass + mass(ii,0);
- partial_arm_cg = partial_arm_cg + link_cg.at(ii);
- }
-
- if(partial_mass!=0){
- X_partial = partial_arm_cg/partial_mass;
-
- Matrix3d screw_rot=Matrix3d::Zero();
- screw_rot(0,1) = -IIIrdcolRot_b_x.at(jj)(2);
- screw_rot(0,2) = IIIrdcolRot_b_x.at(jj)(1);
- screw_rot(1,0) = IIIrdcolRot_b_x.at(jj)(2);
- screw_rot(0,2) = -IIIrdcolRot_b_x.at(jj)(0);
- screw_rot(2,0) = -IIIrdcolRot_b_x.at(jj)(1);
- screw_rot(2,1) = IIIrdcolRot_b_x.at(jj)(0);
-
- Jj_cog.col(jj) = (partial_mass/this->arm_.mass)*screw_rot*X_partial.block(0,0,3,1);
- }
- else Jj_cog.col(jj) = MatrixXd::Zero(3,1);
- }
-
- JG=MatrixXd::Zero(1,4+this->arm_.nj);
- JG.block(0,0,1,4)=MatrixXd::Zero(1,4);
- MatrixXd Jacob_temp(3,this->arm_.nj);
- for (unsigned int ii = 0; ii < this->arm_.nj; ++ii)
- {
- Jacob_temp.col(ii) = Rib * Jj_cog.col(ii);
- }
-
- JG.block(0,4,1,this->arm_.nj)=2*this->ctrl_params_.cog_PGxy.transpose()*Jacob_temp.block(0,0,2,this->arm_.nj);
-
- sigmaG = -(this->ctrl_params_.cog_PGxy.transpose()*this->ctrl_params_.cog_PGxy);
-}
-
-void CQuadarm_Task_Priority_Ctrl::sec_task_jl(MatrixXd& JL,MatrixXd& sigmaL)
-{
- // Get diagonal gain matrix
- MatrixXd AAL=MatrixXd::Zero(this->arm_.nj,this->arm_.nj);
- for (unsigned int ii = 0; ii < this->arm_.nj; ++ii)
- {
- double min,max;
- min=this->arm_.joint_info.at(ii).joint_min;
- max=this->arm_.joint_info.at(ii).joint_max;
- // middle joints
- // this->ctrl_params_.jntlim_pos_des(ii,0) = min+0.5*(max-min);
- AAL(ii,ii) = 1/((max-min)*(max-min));
- }
-
- // Get current arm joint errors
- this->ctrl_params_.jntlim_pos_error = this->arm_.jnt_pos.block(6,0,this->arm_.nj,1)-this->ctrl_params_.jntlim_pos_des;
-
- // Task sigma
- sigmaL = this->ctrl_params_.jntlim_pos_error.transpose()*AAL*this->ctrl_params_.jntlim_pos_error;
-
- // Task Jacobian
- JL = MatrixXd::Zero(1,4+this->arm_.nj);
- JL.block(0,0,1,4) = MatrixXd::Zero(1,4);
- MatrixXd Jtemp = AAL*this->ctrl_params_.jntlim_pos_error;
- JL.block(0,4,1,this->arm_.nj) = -2*Jtemp.transpose();
-}
-
-void CQuadarm_Task_Priority_Ctrl::old_sec_task_vel_unina(){
-
- double ndof = 4+this->arm_.nj;
- this->vel_.old_quadarm= MatrixXd::Zero(ndof,1);
-
- // Kinematic Stabilization
- MatrixXd w_stabil = old_sec_task_stabil();
- this->vel_.old_quadarm = this->vel_.old_quadarm + this->ctrl_params_.stabil_gain*w_stabil;
-
- // Center of Gravity alignement
- MatrixXd w_cg = old_sec_task_cg();
- this->vel_.old_quadarm.block(4,0,5,1) = this->vel_.old_quadarm.block(4,0,5,1) + this->ctrl_params_.cog_gain*w_cg;
-
- // Joint limits
- MatrixXd w_jointlimits = old_sec_task_jointlimits();
- this->vel_.old_quadarm.block(4,0,5,1) = this->vel_.old_quadarm.block(4,0,5,1) + this->ctrl_params_.jntlim_gain*w_jointlimits;
-
-}
-
-MatrixXd CQuadarm_Task_Priority_Ctrl::old_sec_task_stabil(){
-
- // // Stabilize quadrotor ///////////////////////////////////////////////
- MatrixXd w_stabil = MatrixXd::Zero(4+this->arm_.nj,1);
-
- float dq = 0.001;
-
- for (unsigned int i = 0; i < this->arm_.nj; ++i)
- {
- MatrixXd q_old(this->arm_.nj,1),q_new(this->arm_.nj,1);
- MatrixXd angles_old(3,1),angles_new(3,1);
-
- q_old = this->arm_.jnt_pos.block(6,0,this->arm_.nj,1);
- q_old(i,0) = q_old(i,0)-dq;
- q_new = this->arm_.jnt_pos.block(6,0,this->arm_.nj,1);
- q_new(i,0) = q_new(i,0)+dq;
-
- angles_old = old_get_stabil_angles(q_old);
- angles_new = old_get_stabil_angles(q_new);
-
- w_stabil(i,0)=(((angles_new(0)*angles_new(0))+(angles_new(1)*angles_new(1)))-((angles_old(0)*angles_old(0))+(angles_old(1)*angles_old(1))))/dq;
-
- }
-
- return w_stabil;
-}
-
-MatrixXd CQuadarm_Task_Priority_Ctrl::old_get_stabil_angles(const MatrixXd &qa){
-
- for (unsigned int jj = 0; jj < this->arm_.nj; ++jj){
- this->arm_.jnt_pos_kdl.data(jj) = qa(jj,0);
- }
-
- // compute Cartesian pose in realtime
- Frame kdlTarm;
- this->arm_.jnt_to_pose_solver->JntToCart(this->arm_.jnt_pos_kdl, kdlTarm);
- double ro,pit,ya;
- kdlTarm.M.GetEulerZYX(ya,pit,ro);
-
- Matrix4d HT,inv_HT;
- Matrix3d Rarm;
- // euler convention zyx
- Rarm = Eigen::AngleAxisd(ya, Vector3d::UnitZ()) * Eigen::AngleAxisd(pit, Vector3d::UnitY()) * Eigen::AngleAxisd(ro, Vector3d::UnitX());
- HT.block(0,0,3,3) = Rarm;
- HT(0,3) = (double)kdlTarm.p.data[0];
- HT(1,3) = (double)kdlTarm.p.data[1];
- HT(2,3) = (double)kdlTarm.p.data[2];
- HT.row(3) << 0, 0, 0, 1;
-
- // inv_HT = this->T0c_new_*HT.inverse();
- inv_HT = HT.inverse();
-
- MatrixXd angles(3,1);
- Matrix3d Rot;
- Rot = inv_HT.block(0,0,3,3);
- angles = Rot.eulerAngles(2,1,0);
-
- return angles;
-}
-
-
-MatrixXd CQuadarm_Task_Priority_Ctrl::old_sec_task_cg(){
-
- double ndof = 4+this->arm_.nj;
- MatrixXd w_cg = MatrixXd::Zero(this->arm_.nj,1);
- this->vel_.old_quadarm= MatrixXd::Zero(ndof,1);
-
- // Get arm segments
- // num of segments
- unsigned int js = this->arm_.chain.getNrOfSegments();
- vector<Segment> arm_segment(js);
- arm_segment.reserve(js);
-
- for (unsigned int ii = 1; ii < js; ++ii)
- {
- arm_segment.at(ii) = this->arm_.chain.getSegment(ii);
- }
-
- double dq=0.01;
-
- MatrixXd qa_old(this->arm_.nj,1),qa_new(this->arm_.nj,1);
-
- for (unsigned int ii = 0; ii < this->arm_.nj; ++ii){
-
- qa_old = this->arm_.jnt_pos.block(6,0,this->arm_.nj,1);
- qa_new = this->arm_.jnt_pos.block(6,0,this->arm_.nj,1);
- qa_old(ii,0) = qa_old(ii,0)-dq;
- qa_new(ii,0) = qa_new(ii,0)+dq;
-
- MatrixXd rcm_old = old_get_cg_dist(qa_old,arm_segment);
- MatrixXd rcm_new = old_get_cg_dist(qa_new,arm_segment);
-
- w_cg(ii,0)=-(((rcm_new(0,0)*rcm_new(0,0)+rcm_new(1,0)*rcm_new(1,0))-(rcm_old(0,0)*rcm_old(0,0)+rcm_old(1,0)*rcm_old(1,0)))/(2*dq));
- }
-
- return w_cg;
-}
-
-MatrixXd CQuadarm_Task_Priority_Ctrl::old_get_cg_dist(const MatrixXd &qa,const vector<Segment> &arm_segment){
-
-
- MatrixXd rcm = MatrixXd::Zero(3,1);
-
- vector<Frame> joint_pose(this->arm_.nj);
- vector<Matrix4d> Transf(this->arm_.nj);
- vector<MatrixXd> T_base_to_joint(this->arm_.nj+1);
- vector<MatrixXd> dist(this->arm_.nj);
- vector<MatrixXd> link_cg(this->arm_.nj);
- // mass of each link
- MatrixXd mass = MatrixXd::Zero(this->arm_.nj,1);
-
- joint_pose.reserve(this->arm_.nj);
- Transf.reserve(this->arm_.nj);
- T_base_to_joint.reserve(this->arm_.nj+1);
- dist.reserve(this->arm_.nj);
- link_cg.reserve(this->arm_.nj);
-
- double arm_mass = 0;
- MatrixXd arm_cg = MatrixXd::Zero(4,1);
-
- // Get Homogenous transforms of the joints
- for (unsigned int jj = 0; jj < this->arm_.nj; ++jj)
- {
- joint_pose.at(jj) = arm_segment.at(jj+1).pose(qa(jj,0));
- Transf.at(jj) = GetTransform(joint_pose.at(jj));
- mass(jj,0) = arm_segment.at(jj+1).getInertia().getMass();
- }
-
-
- MatrixXd pini(4,1);
- pini.col(0) << 0,0,0,1;
-
- //Fixed transform between base_link and arm_base
- Matrix4d Tbase = Matrix4d::Zero();
- Tbase(0,2) = 1;
- Tbase(1,1) = 1;
- Tbase(2,0) = -1;
- Tbase(3,3) = 1;
-
- T_base_to_joint.at(0) = Tbase*pini;
- T_base_to_joint.at(1) = Tbase*Transf.at(0)*pini;
- T_base_to_joint.at(2) = Tbase*Transf.at(0)*Transf.at(1)*pini;
- T_base_to_joint.at(3) = Tbase*Transf.at(0)*Transf.at(1)*Transf.at(2)*pini;
- T_base_to_joint.at(4) = Tbase*Transf.at(0)*Transf.at(1)*Transf.at(2)*Transf.at(3)*pini;
- T_base_to_joint.at(5) = Tbase*Transf.at(0)*Transf.at(1)*Transf.at(2)*Transf.at(3)*Transf.at(4)*pini;
-
- for (unsigned int jj = 0; jj < this->arm_.nj; ++jj)
- {
- // Distance to the middle of the link jj
- dist.at(jj) = (T_base_to_joint.at(jj+1)-T_base_to_joint.at(jj))/2;
-
- // gravity for the link jj
- link_cg.at(jj) = mass(jj,0)*(T_base_to_joint.at(jj)+dist.at(jj));
-
- // arm CoG
- arm_cg = arm_cg + link_cg.at(jj);
-
- // arm mass
- arm_mass = arm_mass + mass(jj,0);
- }
-
- // vector from arm base to CoG
- rcm = arm_cg/arm_mass;
-
- // *************************************************************
-
- // // CoG r.t. quadrotor body frame
- // MatrixXd rcm_qb(4,1);
- // rcm_qb = Tbase.inverse() * rcm;
-
- // // Rotation between quadrotor body and inertial frames
- // Matrix3d Rot;
- // Rot = AngleAxisd(this->ctrl_params_.q_rollpitch(0,0), Vector3d::UnitZ()) * AngleAxisd(this->ctrl_params_.q_rollpitch(1,0), Vector3d::UnitY()) * AngleAxisd(0, Vector3d::UnitX());
-
- // MatrixXd rcm_2(3,1),rcm_3(3,1), mat1(3,1),mat2(3,1),mat3(3,1), Q1(3,1), Q2(3,1);
-
- // // CoG in quadrotor inertial frame
- // rcm_2 = Rot.transpose()*rcm_qb.block(0,0,3,1);
-
- // // distance of the CoG to the vertical centered line of the quadrotor frame.
- // Vector3d Qs(3,1), vec1(3,1), vec2(3,1);
-
- // Q1 = MatrixXd::Zero(3,1);
- // Q2 = MatrixXd::Zero(3,1);
- // Q1(2,0) = -0.1;
- // Q2(2,0) = -0.5;
-
- // vec1 = rcm_2 - Q1;
- // vec2 = rcm_2 - Q2;
- // Qs = Q2 - Q1;
-
- // mat1 = vec1.cross(vec2);
- // mat2 = mat1.array().abs();
- // mat3 = Qs.array().abs();
-
- // cout << "11" << endl;
- // cout << rcm_2 << endl;
- // cout << "22" << endl;
- // cout << mat3 << endl;
-
-
- // for (unsigned int ii = 0; ii < 3; ++ii)
- // {
- // if (mat3(ii,0)==0) {
- // rcm_3(ii,0)=0;
- // }
- // else {
- // cout << "yeyeyeye" << endl;
- // rcm_3(ii,0) = mat2(ii,0)/mat3(ii,0);
- // }
- // }
-
- // rcm.block(0,0,3,1) = rcm_3;
-
- // cout << "33" << endl;
- // cout << rcm << endl;
-
- // *************************************************************
-
- return rcm;
-}
-
-MatrixXd CQuadarm_Task_Priority_Ctrl::old_sec_task_jointlimits(){
- // Joint Limits ////////////////////////////////////////////////////77
-
- MatrixXd w_jointlimits = MatrixXd::Zero(this->arm_.nj,1);
-
- MatrixXd joint_optim(this->arm_.nj,1), joint_max(this->arm_.nj,1), joint_min(this->arm_.nj,1);
-
- joint_max(0,0) = 0.548;
- joint_min(0,0) = -0.548;
-
- joint_max(1,0) = 1.57;
- joint_min(1,0) = -1.57;
-
- joint_max(2,0) = 5.2;
- joint_min(2,0) = 0;
-
- joint_max(3,0) = 5.2;
- joint_min(3,0) = 0;
-
- joint_max(4,0) = 3.1416;
- joint_min(4,0) = -3.1416;
-
- for (unsigned int jj = 0; jj < this->arm_.nj; ++jj)
- {
- joint_optim(jj,0) = (joint_max(jj,0)+joint_min(jj,0))/2;
-
- double sq_pos = this->arm_.nj * (joint_max(jj,0)-joint_min(jj,0));
-
- w_jointlimits(jj,0)=-((this->arm_.jnt_pos(6+jj,0)-joint_optim(jj,0))/sq_pos);
- }
-
- return w_jointlimits;
-}
-
-void CQuadarm_Task_Priority_Ctrl::old_sec_task_vel_catec()
-{
- //DOF's
- double ndof = 4+this->arm_.nj;
- this->vel_.old_quadarm = MatrixXd::Zero(ndof,1);
-}
-
-MatrixXd CQuadarm_Task_Priority_Ctrl::calcPinv(const MatrixXd &a){
-
- // see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method
- const MatrixXd* m;
- MatrixXd t;
- MatrixXd m_pinv;
-
- // transpose so SVD decomp can work...
- if ( a.rows()<a.cols() ){
- t = a.transpose();
- m = &t;
- } else {
- m = &a;
- }
-
- // SVD
- //JacobiSVD<MatrixXd> svd = m->jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
- JacobiSVD<MatrixXd> svd = m->jacobiSvd(ComputeThinU | ComputeThinV);
- MatrixXd vSingular = svd.singularValues();
- // Build a diagonal matrix with the Inverted Singular values
- // The pseudo inverted singular matrix is easy to compute :
- // is formed by replacing every nonzero entry by its reciprocal (inversing).
- MatrixXd vPseudoInvertedSingular(svd.matrixV().cols(),1);
- for (int iRow =0; iRow<vSingular.rows(); iRow++) {
- // Todo : Put epsilon in parameter
- if ( fabs(vSingular(iRow))<=1e-10 ) {
- vPseudoInvertedSingular(iRow,0)=0.;
- }
- else {
- vPseudoInvertedSingular(iRow,0)=1./vSingular(iRow);
- }
- }
- // A little optimization here
- MatrixXd mAdjointU = svd.matrixU().adjoint().block(0,0,vSingular.rows(),svd.matrixU().adjoint().cols());
- // Pseudo-Inversion : V * S * U'
- m_pinv = (svd.matrixV() * vPseudoInvertedSingular.asDiagonal()) * mAdjointU ;
-
- // transpose back if necessary
- if ( a.rows()<a.cols() )
- return m_pinv.transpose();
-
- return m_pinv;
-}
-
-Matrix4d CQuadarm_Task_Priority_Ctrl::GetTransform(const Frame &f)
-{
-
- double yaw,pitch,roll;
- Matrix3d R;
- Matrix4d T;
-
- f.M.GetEulerZYX(yaw,pitch,roll);
-
- // euler convention zyx
- R = Eigen::AngleAxisd(yaw, Vector3d::UnitZ()) \
- * Eigen::AngleAxisd(pitch, Vector3d::UnitY()) \
- * Eigen::AngleAxisd(roll, Vector3d::UnitX());
- T.block(0,0,3,3)=R;
- T(0,3) = (double)f.p.data[0];
- T(1,3) = (double)f.p.data[1];
- T(2,3) = (double)f.p.data[2];
- T.row(3) << 0, 0, 0, 1;
-
- return T;
-}
-
-void CQuadarm_Task_Priority_Ctrl::write_to_file(const string& folder_name, const string& file_name, const MatrixXd& data, const double& ts)
-{
- // Get home directory
- int myuid;
- passwd *mypasswd;
- string path;
- myuid = getuid();
- mypasswd = getpwuid(myuid);
- path= mypasswd->pw_dir;
- // path+= "/Desktop/Results/";
-
- // Add Folder name
- stringstream folder;
- folder << folder_name;
- path+= folder.str();
-
- // mkdir(path.c_str(), S_IRWXU|S_IRGRP|S_IXGRP);
- // string command = "mkdir -p ";
- // stringstream ss;
- // ss << command << path;
- // string com = ss.str();
- // system(com.c_str());
-
- path+="/";
-
- // Create file
- string FileName;
- FileName = path;
- FileName+= file_name;
- ofstream File;
- File.open(FileName.c_str(), ios::out | ios::app | ios::binary);
-
- // Write time stamp
- File << ts;
- File << ",";
-
- // Write data in Matlab format
- for (int jj = 0; jj < data.cols(); ++jj)
- {
- for (int ii = 0; ii < data.rows()-1; ++ii)
- {
- File << data(ii,jj);
- File << ",";
- }
- }
- // last row
- for (int jj = 0; jj < data.cols()-1; ++jj)
- {
- File << data(data.rows()-1,jj);
- File << ",";
- }
- // last term
- File << data(data.rows()-1,data.cols()-1);
- File << "\n";
- File.close();
-}
\ No newline at end of file
diff --git a/src/.svn/text-base/quadarm_task_priority_ctrl.h.svn-base b/src/.svn/text-base/quadarm_task_priority_ctrl.h.svn-base
deleted file mode 100644
index 34cb85cc6d7a5d3ad0cd9e615d0e6ada642472c1..0000000000000000000000000000000000000000
--- a/src/.svn/text-base/quadarm_task_priority_ctrl.h.svn-base
+++ /dev/null
@@ -1,295 +0,0 @@
-#ifndef _QUADARM_TASK_PRIORITY_CTRL_H
-#define _QUADARM_TASK_PRIORITY_CTRL_H
-
-#include <stdio.h>
-#include <iostream>
-#include <fstream>
-#include <unistd.h>
-#include <string>
-#include <sstream>
-#include <pwd.h>
-#include <math.h>
-#include <Eigen/Dense>
-#include <Eigen/Eigenvalues>
-#include <Eigen/SVD>
-
-#include <sys/stat.h>
-
-// KDL stuff
-#include <kdl/frames.hpp>
-#include <kdl/frames_io.hpp>
-#include <kdl/chain.hpp>
-#include <kdl/chainfksolver.hpp>
-#include <kdl/chainfksolverpos_recursive.hpp>
-#include <kdl/chainiksolver.hpp>
-#include <kdl/chainiksolverpos_nr.hpp>
-#include <kdl/chainiksolvervel_wdls.hpp>
-#include <kdl/frames_io.hpp>
-
-#include <boost/scoped_ptr.hpp>
-#include <boost/shared_ptr.hpp>
-
-using namespace Eigen;
-using namespace KDL;
-using namespace std;
-
-// Homogenous Transforms
-typedef struct{
- Matrix4d tag_in_cam; // Tag in camera frames.
- Matrix4d tag_in_baselink; // Tag in base_link frames.
- Matrix4d cam_in_tip; // Camera in arm tip frames.
- Matrix4d tip_in_baselink; // Arm forward kinematics (from base_link to las arm link frame).
- Matrix4d cam_in_baselink; // Camera in base_link frames.
-}ht;
-
-// Object velocities
-typedef struct{
- MatrixXd cam; // Camera (end-effector).
- MatrixXd quadarm; // Quadrotor and Arm joint velocities.
- MatrixXd old_quadarm; // Old: secondary task velocities.
-}obj_vel;
-
-// Arm joint definition
-typedef struct{
- string link_parent; // Link parent name.
- string link_child; // Link child name.
- double joint_min; // Joint lower limit values.
- double joint_max; // Joint upper limit values.
-}arm_joint;
-
-// Arm definition
-typedef struct{
- Chain chain; // KDL chain.
- double mass; // mass.
- unsigned int nj; // number of joints.
- vector<arm_joint> joint_info; // joints info.
- MatrixXd jnt_pos; // Joint value.
- boost::shared_ptr<ChainFkSolverPos_recursive> jnt_to_pose_solver; // chain solver.
- boost::scoped_ptr<ChainJntToJacSolver> jnt_to_jac_solver; // chain solver.
- JntArray jnt_pos_kdl; // Array of arm positions.
- Jacobian jacobian; // Jacobian.
-}arm;
-
-// Control Law parameters
-typedef struct{
- bool enable_sec_task; // Enable secondary task.
- double dt; // time interval.
- double stabil_gain; // Gain of kinematics stabilization secondary task.
- MatrixXd lambda_robot; // Robot proportional gains.
- MatrixXd vs_vel; // Primary task velocity.
- double vs_alpha_min; // Alpha value for gain matrix pseudo inverse.
- MatrixXd vs_delta_gain; // Delta values (min and max) for gain matrix pseudo inverse.
- double cog_gain; // Gain of CoG alignement secondary task.
- MatrixXd cog_vel; // Secondary task velocity.
- MatrixXd cog_PGxy; // X and Y fo the CoG r.t. Quadrotor body inertial frame.
- MatrixXd cog_arm; // Arm Center of Gravity r.t. quadrotor body frame.
- double jntlim_gain; // Gain of joint limits secondary task.
- MatrixXd jntlim_vel; // Joint limit task velocity.
- MatrixXd jntlim_pos_des; // Desired arm position for secondary task (avoiding joint limits).
- MatrixXd jntlim_pos_error; // Arm joint errors between current and secondary task desired joint positions
- MatrixXd q_rollpitch; // Quadrotor roll and pitch angles.
- MatrixXd v_rollpitch; // Quadrotor roll and pitch angular velocities.
-}ctrl_params;
-
-
-class CQuadarm_Task_Priority_Ctrl
-{
-
- private:
-
- ht T_; // Homogenous Transforms
-
- obj_vel vel_; // Robot velocities
-
- MatrixXd Jqa_; // Quadrotor and arm jacobian.
-
- double target_dist_; // Euclidean distance to target.
-
- arm arm_; // Arm
-
- ctrl_params ctrl_params_; // Control Law parameters
-
- /**
- * \brief Compute Quadrotor and Arm Kinematics
- *
- * Compute the quadrotor and arm jacobian and the position of the quadrotor using forward kinematics
- *
- */
- void qa_kinematics();
-
- /**
- * \brief Compute Quadrotor Jacobian
- *
- * Compute the quadrotor Jacobian
- *
- */
- MatrixXd quadrotor_jacobian();
-
- /**
- * \brief Compute Arm Jacobian
- *
- * Compute the arm Jacobian
- *
- */
- MatrixXd arm_jacobian();
-
- /**
- * \brief Compute Arm Forward Kinematics
- *
- * Compute the arm Forward Kinematics
- *
- */
- Matrix4d arm_f_kinematics();
-
- /**
- * \brief Compute the 9DOF control law
- *
- * Compute the motions of the quadrotor and arm joints
- *
- */
- void qa_control();
-
- /**
- * \brief Get weighted generalized Jacobian inverse
- *
- * Compute the jacobian inverse weighted depending on target distance
- *
- */
- void weight_inverse(const MatrixXd& J, MatrixXd& J_inverse);
-
- /**
- * \brief Secondary task gravity alignement
- *
- * Compute vector of the secondary task to vertically align the arm center of gravity
- *
- * with the quadrotor gravitational vector
- *
- */
- void sec_task_cog(MatrixXd& JG,MatrixXd& sigmaG);
-
- /**
- * \brief Secondary task Arm joint limits avoidance
- *
- * Compute vector of the secondary task to avoid arm joint limits
- *
- */
- void sec_task_jl(MatrixXd& JG,MatrixXd& sigmaG);
-
- /**
- * \brief Compute Desired pose velocities at the jacobian null space (over-determined system)
- *
- * Compute the vector of motion of the jacobian null space
- *
- */
- void old_sec_task_vel_unina();
-
- /**
- * \brief Compute Desired pose velocities at the jacobian null space (over-determined system)
- *
- * Compute the vector of motion of the jacobian null space
- *
- */
- void old_sec_task_vel_catec();
-
- /**
- * \brief Secondary task kinematic stabilization
- *
- * Compute those solutions that accomplish the primary task and also that reaches horizontal arm base.
- *
- */
- MatrixXd old_sec_task_stabil();
-
- /**
- * \brief Get Arm base angles due to arm kinematics
- *
- * Compute the arm kinematics to know the produced angles in the arm base.
- *
- */
- MatrixXd old_get_stabil_angles(const MatrixXd &qa);
-
- /**
- * \brief OLD Secondary task gravity alignement
- *
- * Compute vector of the secondary task to vertically align the arm center of gravity
- *
- * with the quadrotor gravitational vector
- *
- */
- MatrixXd old_sec_task_cg();
-
- /**
- * \brief OLD Secondary task Arm joint limits avoidance
- *
- * Compute vector of the secondary task to avoid arm joint limits
- *
- */
- MatrixXd old_sec_task_jointlimits();
-
- /**
- * \brief Get arm's center of gravity distance
- *
- * Get arm's center of gravity distance
- *
- */
- MatrixXd old_get_cg_dist(const MatrixXd &qa,const vector<Segment> &arm_segment);
-
- /**
- * \bried Matrix pseudo-inverse
- *
- * Compute the matrix pseudo-inverse using SVD
- */
- MatrixXd calcPinv(const MatrixXd &a);
-
- /**
- * \bried KDL Frame to Homogenous transform
- *
- * Compute the convertion from KDL Frame to Homogenous transform (Eigen Matrix 4f)
- */
- Matrix4d GetTransform(const Frame &f);
-
- public:
-
- /**
- * \brief Constructor
- *
- * Quadarm Task Priority Control Class constructor.
- *
- */
- CQuadarm_Task_Priority_Ctrl();
-
- /**
- * \brief Destructor
- *
- * Quadarm Task Priority Control Class destructor.
- *
- */
- ~CQuadarm_Task_Priority_Ctrl();
-
- /**
- * \brief Quadarm Task Priority Control Main API Function
- *
- * Main public function call of Quadarm Task Priority Control.
- *
- */
- void quadarm_task_priority_ctrl(const MatrixXd& cam_vel,
- const Matrix4d& Tcam_in_tip,
- const Matrix4d& Ttag_in_cam,
- const double& target_dist,
- const Chain& kdl_chain,
- const vector<arm_joint> joint_info,
- ctrl_params& ctrl_params,
- MatrixXd& joint_pos,
- MatrixXd& joint_vels);
-
-
- /**
- * \brief Write to file
- *
- * Write to file some vars to plot them externally. Input data must be an Eigen MatrixX
- *
- */
- void write_to_file(const string& folder_name, const string& file_name, const MatrixXd& data, const double& ts);
-
-};
-
-#endif