Squashed 'third_party/ct/' content from commit 0048d02
Change-Id: Ia7e5360cbb414f92ce4f118bd9613ea23597db52
git-subtree-dir: third_party/ct
git-subtree-split: 0048d027531b6cf1ea730da17b68a0b7ef9070b1
diff --git a/ct_rbd/CMakeLists.txt b/ct_rbd/CMakeLists.txt
new file mode 100644
index 0000000..5b83222
--- /dev/null
+++ b/ct_rbd/CMakeLists.txt
@@ -0,0 +1,127 @@
+cmake_minimum_required (VERSION 2.6)
+
+include(${CMAKE_CURRENT_SOURCE_DIR}/../ct/cmake/compilerSettings.cmake)
+include(${CMAKE_CURRENT_SOURCE_DIR}/../ct/cmake/explicitTemplateHelpers.cmake)
+include(${CMAKE_CURRENT_SOURCE_DIR}/../ct/cmake/clang-cxx-dev-tools.cmake)
+
+project (ct_rbd)
+
+SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wfatal-errors -std=c++11 -Wall")
+SET(CMAKE_EXPORT_COMPILE_COMMANDS TRUE)
+
+find_package(catkin REQUIRED ct_core ct_optcon)
+find_package(kindr)
+
+
+## assemble list of libraries that contain prespecified templates
+ct_configure_explicit_templates("${CMAKE_CURRENT_SOURCE_DIR}/../ct/config/explicit_templates.cfg" "${CMAKE_CURRENT_SOURCE_DIR}/prespec/" "ct_rbd")
+message(WARNING "CT RBD: Compiling the following explict template libraries: ${PRESPEC_LIB_NAMES}")
+
+#used later for clang-tidy
+set(INC_DIRS ${catkin_INCLUDE_DIRS} ${kindr_INCLUDE_DIRS} "${CMAKE_CURRENT_SOURCE_DIR}/include" "${CMAKE_CURRENT_SOURCE_DIR}/include/ct/rbd")
+
+
+catkin_package(
+ INCLUDE_DIRS
+ ${kindr_INCLUDE_DIRS}
+ include
+ include/ct/rbd
+ include/ct/rbd/robot/kinematics/ikfast
+ test
+ LIBRARIES
+ ${PRESPEC_LIB_NAMES}
+ CATKIN_DEPENDS
+ ct_core
+ ct_optcon
+ kindr
+)
+
+include_directories(
+ ${catkin_INCLUDE_DIRS}
+ ${kindr_INCLUDE_DIRS}
+ include/ct/rbd
+ include/ct/rbd/robot/kinematics/ikfast
+ test/models/testhyq/generated
+ include
+ test
+)
+
+
+# add libraries for explicit template
+ct_add_explicit_template_libs()
+
+
+## TESTING
+
+catkin_add_gtest(RigidBodyPoseTest test/state/RigidBodyPoseTest.cpp)
+target_link_libraries(RigidBodyPoseTest ${catkin_LIBRARIES})
+
+catkin_add_gtest(JointStateTest test/state/JointStateTest.cpp)
+target_link_libraries(JointStateTest ${catkin_LIBRARIES})
+
+catkin_add_gtest(RobCoGenContainerTest test/robot/robcogen/RobCoGenContainerTest.cpp)
+target_link_libraries(RobCoGenContainerTest ${catkin_LIBRARIES})
+
+catkin_add_gtest(KinematicsTest test/robot/kinematics/KinematicsTest.cpp)
+target_link_libraries(KinematicsTest ${catkin_LIBRARIES})
+
+catkin_add_gtest(KinematicsTestAd test/robot/kinematics/KinematicsTestAd.cpp)
+target_link_libraries(KinematicsTestAd ${catkin_LIBRARIES})
+
+catkin_add_gtest(OperationalSpaceTest test/operationalSpace/OperationalSpaceTest.cpp)
+target_link_libraries(OperationalSpaceTest ${catkin_LIBRARIES})
+
+catkin_add_gtest(DataMapTests test/robot/dynamics/DataMapTests.cpp)
+target_link_libraries(DataMapTests ${catkin_LIBRARIES})
+
+catkin_add_gtest(DynamicsTest test/robot/dynamics/DynamicsTests.cpp)
+target_link_libraries(DynamicsTest ${catkin_LIBRARIES})
+
+catkin_add_gtest(DynamicsTestFixBase test/robot/dynamics/DynamicsTestsFixBase.cpp)
+target_link_libraries(DynamicsTestFixBase ${catkin_LIBRARIES})
+
+catkin_add_gtest(FloatingBaseFDSystemTest test/systems/FloatingBaseFDSystemTest.cpp)
+target_link_libraries(FloatingBaseFDSystemTest ${catkin_LIBRARIES})
+
+catkin_add_gtest(FixBaseFDSystemTest test/systems/FixBaseFDSystemTest.cpp)
+target_link_libraries(FixBaseFDSystemTest ${catkin_LIBRARIES})
+
+catkin_add_gtest(ProjectedFDSystemTest test/systems/ProjectedFDSystemTest.cpp)
+target_link_libraries(ProjectedFDSystemTest ${catkin_LIBRARIES})
+
+catkin_add_gtest(RBDLinearizerTest test/systems/linear/RBDLinearizerTest.cpp)
+target_link_libraries(RBDLinearizerTest ${catkin_LIBRARIES})
+
+catkin_add_gtest(EEKinematicsTest test/robot/kinematics/EEKinematicsTest.cpp)
+target_link_libraries(EEKinematicsTest ${catkin_LIBRARIES})
+
+catkin_add_gtest(EEContactModelTest test/physics/EEContactModelTest.cpp)
+target_link_libraries(EEContactModelTest ${catkin_LIBRARIES})
+
+# todo make unit test again
+catkin_add_gtest(TaskSpaceCfTest test/robot/costfunction/TaskspaceCostFunctionTest.cpp)
+target_link_libraries(TaskSpaceCfTest ${catkin_LIBRARIES})
+catkin_add_gtest(rbdJITtests test/robot/costfunction/rbdJITtests.cpp)
+target_link_libraries(rbdJITtests ${catkin_LIBRARIES})
+catkin_add_gtest(kindrJITtest test/robot/costfunction/kindrJITtest.cpp)
+target_link_libraries(kindrJITtest ${catkin_LIBRARIES})
+
+#catkin_add_gtest(ControllerTests test/robot/control/ControllerTests.cpp)
+#target_link_libraries(ControllerTests ${catkin_LIBRARIES})
+
+find_package(Doxygen)
+if(DOXYGEN_FOUND)
+ set(doxyfile_in ${CMAKE_CURRENT_SOURCE_DIR}/doc/ct_rbd.doxyfile)
+ set(doxyfile ${CMAKE_CURRENT_SOURCE_DIR}/Doxyfile)
+
+ configure_file(${doxyfile_in} ${doxyfile} @ONLY)
+
+ add_custom_target(doc
+ COMMAND ${CMAKE_COMMAND} -E echo_append "Building API Documentation..."
+ COMMAND ${DOXYGEN_EXECUTABLE} ${doxyfile}
+ WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/doc
+ COMMAND ${CMAKE_COMMAND} -E echo_append "API Documentation built in ${CMAKE_CURRENT_SOURCE_DIR}/doc"
+ VERBATIM)
+endif()
+
+ct_configure_clang_tidy(INC_DIRS)
diff --git a/ct_rbd/Doxyfile b/ct_rbd/Doxyfile
new file mode 100644
index 0000000..25cd999
--- /dev/null
+++ b/ct_rbd/Doxyfile
@@ -0,0 +1,2316 @@
+# Doxyfile 1.8.7
+
+# This file describes the settings to be used by the documentation system
+# doxygen (www.doxygen.org) for a project.
+#
+# All text after a double hash (##) is considered a comment and is placed in
+# front of the TAG it is preceding.
+#
+# All text after a single hash (#) is considered a comment and will be ignored.
+# The format is:
+# TAG = value [value, ...]
+# For lists, items can also be appended using:
+# TAG += value [value, ...]
+# Values that contain spaces should be placed between quotes (\" \").
+
+#---------------------------------------------------------------------------
+# Project related configuration options
+#---------------------------------------------------------------------------
+
+# This tag specifies the encoding used for all characters in the config file
+# that follow. The default is UTF-8 which is also the encoding used for all text
+# before the first occurrence of this tag. Doxygen uses libiconv (or the iconv
+# built into libc) for the transcoding. See http://www.gnu.org/software/libiconv
+# for the list of possible encodings.
+# The default value is: UTF-8.
+
+DOXYFILE_ENCODING = UTF-8
+
+# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by
+# double-quotes, unless you are using Doxywizard) that should identify the
+# project for which the documentation is generated. This name is used in the
+# title of most generated pages and in a few other places.
+# The default value is: My Project.
+
+PROJECT_NAME = ct_rbd
+
+# The PROJECT_NUMBER tag can be used to enter a project or revision number. This
+# could be handy for archiving the generated documentation or if some version
+# control system is used.
+
+PROJECT_NUMBER = v2.1
+
+# Using the PROJECT_BRIEF tag one can provide an optional one line description
+# for a project that appears at the top of each page and should give viewer a
+# quick idea about the purpose of the project. Keep the description short.
+
+PROJECT_BRIEF = "Control Toolbox - Rigid Body Dynamics Module"
+
+# With the PROJECT_LOGO tag one can specify an logo or icon that is included in
+# the documentation. The maximum height of the logo should not exceed 55 pixels
+# and the maximum width should not exceed 200 pixels. Doxygen will copy the logo
+# to the output directory.
+
+PROJECT_LOGO =
+
+# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path
+# into which the generated documentation will be written. If a relative path is
+# entered, it will be relative to the location where doxygen was started. If
+# left blank the current directory will be used.
+
+OUTPUT_DIRECTORY = .
+
+# If the CREATE_SUBDIRS tag is set to YES, then doxygen will create 4096 sub-
+# directories (in 2 levels) under the output directory of each output format and
+# will distribute the generated files over these directories. Enabling this
+# option can be useful when feeding doxygen a huge amount of source files, where
+# putting all generated files in the same directory would otherwise causes
+# performance problems for the file system.
+# The default value is: NO.
+
+CREATE_SUBDIRS = NO
+
+# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII
+# characters to appear in the names of generated files. If set to NO, non-ASCII
+# characters will be escaped, for example _xE3_x81_x84 will be used for Unicode
+# U+3044.
+# The default value is: NO.
+
+ALLOW_UNICODE_NAMES = NO
+
+# The OUTPUT_LANGUAGE tag is used to specify the language in which all
+# documentation generated by doxygen is written. Doxygen will use this
+# information to generate all constant output in the proper language.
+# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese,
+# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States),
+# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian,
+# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages),
+# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian,
+# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian,
+# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish,
+# Ukrainian and Vietnamese.
+# The default value is: English.
+
+OUTPUT_LANGUAGE = English
+
+# If the BRIEF_MEMBER_DESC tag is set to YES doxygen will include brief member
+# descriptions after the members that are listed in the file and class
+# documentation (similar to Javadoc). Set to NO to disable this.
+# The default value is: YES.
+
+BRIEF_MEMBER_DESC = YES
+
+# If the REPEAT_BRIEF tag is set to YES doxygen will prepend the brief
+# description of a member or function before the detailed description
+#
+# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the
+# brief descriptions will be completely suppressed.
+# The default value is: YES.
+
+REPEAT_BRIEF = YES
+
+# This tag implements a quasi-intelligent brief description abbreviator that is
+# used to form the text in various listings. Each string in this list, if found
+# as the leading text of the brief description, will be stripped from the text
+# and the result, after processing the whole list, is used as the annotated
+# text. Otherwise, the brief description is used as-is. If left blank, the
+# following values are used ($name is automatically replaced with the name of
+# the entity):The $name class, The $name widget, The $name file, is, provides,
+# specifies, contains, represents, a, an and the.
+
+ABBREVIATE_BRIEF =
+
+# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then
+# doxygen will generate a detailed section even if there is only a brief
+# description.
+# The default value is: NO.
+
+ALWAYS_DETAILED_SEC = NO
+
+# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all
+# inherited members of a class in the documentation of that class as if those
+# members were ordinary class members. Constructors, destructors and assignment
+# operators of the base classes will not be shown.
+# The default value is: NO.
+
+INLINE_INHERITED_MEMB = NO
+
+# If the FULL_PATH_NAMES tag is set to YES doxygen will prepend the full path
+# before files name in the file list and in the header files. If set to NO the
+# shortest path that makes the file name unique will be used
+# The default value is: YES.
+
+FULL_PATH_NAMES = YES
+
+# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path.
+# Stripping is only done if one of the specified strings matches the left-hand
+# part of the path. The tag can be used to show relative paths in the file list.
+# If left blank the directory from which doxygen is run is used as the path to
+# strip.
+#
+# Note that you can specify absolute paths here, but also relative paths, which
+# will be relative from the directory where doxygen is started.
+# This tag requires that the tag FULL_PATH_NAMES is set to YES.
+
+STRIP_FROM_PATH =
+
+# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the
+# path mentioned in the documentation of a class, which tells the reader which
+# header file to include in order to use a class. If left blank only the name of
+# the header file containing the class definition is used. Otherwise one should
+# specify the list of include paths that are normally passed to the compiler
+# using the -I flag.
+
+STRIP_FROM_INC_PATH =
+
+# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but
+# less readable) file names. This can be useful is your file systems doesn't
+# support long names like on DOS, Mac, or CD-ROM.
+# The default value is: NO.
+
+SHORT_NAMES = NO
+
+# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the
+# first line (until the first dot) of a Javadoc-style comment as the brief
+# description. If set to NO, the Javadoc-style will behave just like regular Qt-
+# style comments (thus requiring an explicit @brief command for a brief
+# description.)
+# The default value is: NO.
+
+JAVADOC_AUTOBRIEF = NO
+
+# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first
+# line (until the first dot) of a Qt-style comment as the brief description. If
+# set to NO, the Qt-style will behave just like regular Qt-style comments (thus
+# requiring an explicit \brief command for a brief description.)
+# The default value is: NO.
+
+QT_AUTOBRIEF = NO
+
+# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a
+# multi-line C++ special comment block (i.e. a block of //! or /// comments) as
+# a brief description. This used to be the default behavior. The new default is
+# to treat a multi-line C++ comment block as a detailed description. Set this
+# tag to YES if you prefer the old behavior instead.
+#
+# Note that setting this tag to YES also means that rational rose comments are
+# not recognized any more.
+# The default value is: NO.
+
+MULTILINE_CPP_IS_BRIEF = NO
+
+# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the
+# documentation from any documented member that it re-implements.
+# The default value is: YES.
+
+INHERIT_DOCS = YES
+
+# If the SEPARATE_MEMBER_PAGES tag is set to YES, then doxygen will produce a
+# new page for each member. If set to NO, the documentation of a member will be
+# part of the file/class/namespace that contains it.
+# The default value is: NO.
+
+SEPARATE_MEMBER_PAGES = NO
+
+# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen
+# uses this value to replace tabs by spaces in code fragments.
+# Minimum value: 1, maximum value: 16, default value: 4.
+
+TAB_SIZE = 4
+
+# This tag can be used to specify a number of aliases that act as commands in
+# the documentation. An alias has the form:
+# name=value
+# For example adding
+# "sideeffect=@par Side Effects:\n"
+# will allow you to put the command \sideeffect (or @sideeffect) in the
+# documentation, which will result in a user-defined paragraph with heading
+# "Side Effects:". You can put \n's in the value part of an alias to insert
+# newlines.
+
+ALIASES =
+
+# This tag can be used to specify a number of word-keyword mappings (TCL only).
+# A mapping has the form "name=value". For example adding "class=itcl::class"
+# will allow you to use the command class in the itcl::class meaning.
+
+TCL_SUBST =
+
+# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources
+# only. Doxygen will then generate output that is more tailored for C. For
+# instance, some of the names that are used will be different. The list of all
+# members will be omitted, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_FOR_C = NO
+
+# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or
+# Python sources only. Doxygen will then generate output that is more tailored
+# for that language. For instance, namespaces will be presented as packages,
+# qualified scopes will look different, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_JAVA = NO
+
+# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran
+# sources. Doxygen will then generate output that is tailored for Fortran.
+# The default value is: NO.
+
+OPTIMIZE_FOR_FORTRAN = NO
+
+# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL
+# sources. Doxygen will then generate output that is tailored for VHDL.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_VHDL = NO
+
+# Doxygen selects the parser to use depending on the extension of the files it
+# parses. With this tag you can assign which parser to use for a given
+# extension. Doxygen has a built-in mapping, but you can override or extend it
+# using this tag. The format is ext=language, where ext is a file extension, and
+# language is one of the parsers supported by doxygen: IDL, Java, Javascript,
+# C#, C, C++, D, PHP, Objective-C, Python, Fortran (fixed format Fortran:
+# FortranFixed, free formatted Fortran: FortranFree, unknown formatted Fortran:
+# Fortran. In the later case the parser tries to guess whether the code is fixed
+# or free formatted code, this is the default for Fortran type files), VHDL. For
+# instance to make doxygen treat .inc files as Fortran files (default is PHP),
+# and .f files as C (default is Fortran), use: inc=Fortran f=C.
+#
+# Note For files without extension you can use no_extension as a placeholder.
+#
+# Note that for custom extensions you also need to set FILE_PATTERNS otherwise
+# the files are not read by doxygen.
+
+EXTENSION_MAPPING =
+
+# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments
+# according to the Markdown format, which allows for more readable
+# documentation. See http://daringfireball.net/projects/markdown/ for details.
+# The output of markdown processing is further processed by doxygen, so you can
+# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in
+# case of backward compatibilities issues.
+# The default value is: YES.
+
+MARKDOWN_SUPPORT = YES
+
+# When enabled doxygen tries to link words that correspond to documented
+# classes, or namespaces to their corresponding documentation. Such a link can
+# be prevented in individual cases by by putting a % sign in front of the word
+# or globally by setting AUTOLINK_SUPPORT to NO.
+# The default value is: YES.
+
+AUTOLINK_SUPPORT = YES
+
+# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want
+# to include (a tag file for) the STL sources as input, then you should set this
+# tag to YES in order to let doxygen match functions declarations and
+# definitions whose arguments contain STL classes (e.g. func(std::string);
+# versus func(std::string) {}). This also make the inheritance and collaboration
+# diagrams that involve STL classes more complete and accurate.
+# The default value is: NO.
+
+BUILTIN_STL_SUPPORT = NO
+
+# If you use Microsoft's C++/CLI language, you should set this option to YES to
+# enable parsing support.
+# The default value is: NO.
+
+CPP_CLI_SUPPORT = NO
+
+# Set the SIP_SUPPORT tag to YES if your project consists of sip (see:
+# http://www.riverbankcomputing.co.uk/software/sip/intro) sources only. Doxygen
+# will parse them like normal C++ but will assume all classes use public instead
+# of private inheritance when no explicit protection keyword is present.
+# The default value is: NO.
+
+SIP_SUPPORT = NO
+
+# For Microsoft's IDL there are propget and propput attributes to indicate
+# getter and setter methods for a property. Setting this option to YES will make
+# doxygen to replace the get and set methods by a property in the documentation.
+# This will only work if the methods are indeed getting or setting a simple
+# type. If this is not the case, or you want to show the methods anyway, you
+# should set this option to NO.
+# The default value is: YES.
+
+IDL_PROPERTY_SUPPORT = YES
+
+# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC
+# tag is set to YES, then doxygen will reuse the documentation of the first
+# member in the group (if any) for the other members of the group. By default
+# all members of a group must be documented explicitly.
+# The default value is: NO.
+
+DISTRIBUTE_GROUP_DOC = YES
+
+# Set the SUBGROUPING tag to YES to allow class member groups of the same type
+# (for instance a group of public functions) to be put as a subgroup of that
+# type (e.g. under the Public Functions section). Set it to NO to prevent
+# subgrouping. Alternatively, this can be done per class using the
+# \nosubgrouping command.
+# The default value is: YES.
+
+SUBGROUPING = YES
+
+# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions
+# are shown inside the group in which they are included (e.g. using \ingroup)
+# instead of on a separate page (for HTML and Man pages) or section (for LaTeX
+# and RTF).
+#
+# Note that this feature does not work in combination with
+# SEPARATE_MEMBER_PAGES.
+# The default value is: NO.
+
+INLINE_GROUPED_CLASSES = NO
+
+# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions
+# with only public data fields or simple typedef fields will be shown inline in
+# the documentation of the scope in which they are defined (i.e. file,
+# namespace, or group documentation), provided this scope is documented. If set
+# to NO, structs, classes, and unions are shown on a separate page (for HTML and
+# Man pages) or section (for LaTeX and RTF).
+# The default value is: NO.
+
+INLINE_SIMPLE_STRUCTS = NO
+
+# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or
+# enum is documented as struct, union, or enum with the name of the typedef. So
+# typedef struct TypeS {} TypeT, will appear in the documentation as a struct
+# with name TypeT. When disabled the typedef will appear as a member of a file,
+# namespace, or class. And the struct will be named TypeS. This can typically be
+# useful for C code in case the coding convention dictates that all compound
+# types are typedef'ed and only the typedef is referenced, never the tag name.
+# The default value is: NO.
+
+TYPEDEF_HIDES_STRUCT = NO
+
+# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This
+# cache is used to resolve symbols given their name and scope. Since this can be
+# an expensive process and often the same symbol appears multiple times in the
+# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small
+# doxygen will become slower. If the cache is too large, memory is wasted. The
+# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range
+# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536
+# symbols. At the end of a run doxygen will report the cache usage and suggest
+# the optimal cache size from a speed point of view.
+# Minimum value: 0, maximum value: 9, default value: 0.
+
+LOOKUP_CACHE_SIZE = 0
+
+#---------------------------------------------------------------------------
+# Build related configuration options
+#---------------------------------------------------------------------------
+
+# If the EXTRACT_ALL tag is set to YES doxygen will assume all entities in
+# documentation are documented, even if no documentation was available. Private
+# class members and static file members will be hidden unless the
+# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES.
+# Note: This will also disable the warnings about undocumented members that are
+# normally produced when WARNINGS is set to YES.
+# The default value is: NO.
+
+EXTRACT_ALL = YES
+
+# If the EXTRACT_PRIVATE tag is set to YES all private members of a class will
+# be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PRIVATE = NO
+
+# If the EXTRACT_PACKAGE tag is set to YES all members with package or internal
+# scope will be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PACKAGE = NO
+
+# If the EXTRACT_STATIC tag is set to YES all static members of a file will be
+# included in the documentation.
+# The default value is: NO.
+
+EXTRACT_STATIC = NO
+
+# If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) defined
+# locally in source files will be included in the documentation. If set to NO
+# only classes defined in header files are included. Does not have any effect
+# for Java sources.
+# The default value is: YES.
+
+EXTRACT_LOCAL_CLASSES = YES
+
+# This flag is only useful for Objective-C code. When set to YES local methods,
+# which are defined in the implementation section but not in the interface are
+# included in the documentation. If set to NO only methods in the interface are
+# included.
+# The default value is: NO.
+
+EXTRACT_LOCAL_METHODS = NO
+
+# If this flag is set to YES, the members of anonymous namespaces will be
+# extracted and appear in the documentation as a namespace called
+# 'anonymous_namespace{file}', where file will be replaced with the base name of
+# the file that contains the anonymous namespace. By default anonymous namespace
+# are hidden.
+# The default value is: NO.
+
+EXTRACT_ANON_NSPACES = NO
+
+# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all
+# undocumented members inside documented classes or files. If set to NO these
+# members will be included in the various overviews, but no documentation
+# section is generated. This option has no effect if EXTRACT_ALL is enabled.
+# The default value is: NO.
+
+HIDE_UNDOC_MEMBERS = YES
+
+# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all
+# undocumented classes that are normally visible in the class hierarchy. If set
+# to NO these classes will be included in the various overviews. This option has
+# no effect if EXTRACT_ALL is enabled.
+# The default value is: NO.
+
+HIDE_UNDOC_CLASSES = YES
+
+# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend
+# (class|struct|union) declarations. If set to NO these declarations will be
+# included in the documentation.
+# The default value is: NO.
+
+HIDE_FRIEND_COMPOUNDS = NO
+
+# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any
+# documentation blocks found inside the body of a function. If set to NO these
+# blocks will be appended to the function's detailed documentation block.
+# The default value is: NO.
+
+HIDE_IN_BODY_DOCS = NO
+
+# The INTERNAL_DOCS tag determines if documentation that is typed after a
+# \internal command is included. If the tag is set to NO then the documentation
+# will be excluded. Set it to YES to include the internal documentation.
+# The default value is: NO.
+
+INTERNAL_DOCS = NO
+
+# If the CASE_SENSE_NAMES tag is set to NO then doxygen will only generate file
+# names in lower-case letters. If set to YES upper-case letters are also
+# allowed. This is useful if you have classes or files whose names only differ
+# in case and if your file system supports case sensitive file names. Windows
+# and Mac users are advised to set this option to NO.
+# The default value is: system dependent.
+
+CASE_SENSE_NAMES = YES
+
+# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with
+# their full class and namespace scopes in the documentation. If set to YES the
+# scope will be hidden.
+# The default value is: NO.
+
+HIDE_SCOPE_NAMES = NO
+
+# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of
+# the files that are included by a file in the documentation of that file.
+# The default value is: YES.
+
+SHOW_INCLUDE_FILES = YES
+
+# If the SHOW_GROUPED_MEMB_INC tag is set to YES then Doxygen will add for each
+# grouped member an include statement to the documentation, telling the reader
+# which file to include in order to use the member.
+# The default value is: NO.
+
+SHOW_GROUPED_MEMB_INC = NO
+
+# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include
+# files with double quotes in the documentation rather than with sharp brackets.
+# The default value is: NO.
+
+FORCE_LOCAL_INCLUDES = NO
+
+# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the
+# documentation for inline members.
+# The default value is: YES.
+
+INLINE_INFO = YES
+
+# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the
+# (detailed) documentation of file and class members alphabetically by member
+# name. If set to NO the members will appear in declaration order.
+# The default value is: YES.
+
+SORT_MEMBER_DOCS = NO
+
+# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief
+# descriptions of file, namespace and class members alphabetically by member
+# name. If set to NO the members will appear in declaration order. Note that
+# this will also influence the order of the classes in the class list.
+# The default value is: NO.
+
+SORT_BRIEF_DOCS = NO
+
+# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the
+# (brief and detailed) documentation of class members so that constructors and
+# destructors are listed first. If set to NO the constructors will appear in the
+# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS.
+# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief
+# member documentation.
+# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting
+# detailed member documentation.
+# The default value is: NO.
+
+SORT_MEMBERS_CTORS_1ST = NO
+
+# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy
+# of group names into alphabetical order. If set to NO the group names will
+# appear in their defined order.
+# The default value is: NO.
+
+SORT_GROUP_NAMES = NO
+
+# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by
+# fully-qualified names, including namespaces. If set to NO, the class list will
+# be sorted only by class name, not including the namespace part.
+# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES.
+# Note: This option applies only to the class list, not to the alphabetical
+# list.
+# The default value is: NO.
+
+SORT_BY_SCOPE_NAME = NO
+
+# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper
+# type resolution of all parameters of a function it will reject a match between
+# the prototype and the implementation of a member function even if there is
+# only one candidate or it is obvious which candidate to choose by doing a
+# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still
+# accept a match between prototype and implementation in such cases.
+# The default value is: NO.
+
+STRICT_PROTO_MATCHING = NO
+
+# The GENERATE_TODOLIST tag can be used to enable ( YES) or disable ( NO) the
+# todo list. This list is created by putting \todo commands in the
+# documentation.
+# The default value is: YES.
+
+GENERATE_TODOLIST = YES
+
+# The GENERATE_TESTLIST tag can be used to enable ( YES) or disable ( NO) the
+# test list. This list is created by putting \test commands in the
+# documentation.
+# The default value is: YES.
+
+GENERATE_TESTLIST = YES
+
+# The GENERATE_BUGLIST tag can be used to enable ( YES) or disable ( NO) the bug
+# list. This list is created by putting \bug commands in the documentation.
+# The default value is: YES.
+
+GENERATE_BUGLIST = YES
+
+# The GENERATE_DEPRECATEDLIST tag can be used to enable ( YES) or disable ( NO)
+# the deprecated list. This list is created by putting \deprecated commands in
+# the documentation.
+# The default value is: YES.
+
+GENERATE_DEPRECATEDLIST= YES
+
+# The ENABLED_SECTIONS tag can be used to enable conditional documentation
+# sections, marked by \if <section_label> ... \endif and \cond <section_label>
+# ... \endcond blocks.
+
+ENABLED_SECTIONS =
+
+# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the
+# initial value of a variable or macro / define can have for it to appear in the
+# documentation. If the initializer consists of more lines than specified here
+# it will be hidden. Use a value of 0 to hide initializers completely. The
+# appearance of the value of individual variables and macros / defines can be
+# controlled using \showinitializer or \hideinitializer command in the
+# documentation regardless of this setting.
+# Minimum value: 0, maximum value: 10000, default value: 30.
+
+MAX_INITIALIZER_LINES = 30
+
+# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at
+# the bottom of the documentation of classes and structs. If set to YES the list
+# will mention the files that were used to generate the documentation.
+# The default value is: YES.
+
+SHOW_USED_FILES = YES
+
+# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This
+# will remove the Files entry from the Quick Index and from the Folder Tree View
+# (if specified).
+# The default value is: YES.
+
+SHOW_FILES = YES
+
+# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces
+# page. This will remove the Namespaces entry from the Quick Index and from the
+# Folder Tree View (if specified).
+# The default value is: YES.
+
+SHOW_NAMESPACES = YES
+
+# The FILE_VERSION_FILTER tag can be used to specify a program or script that
+# doxygen should invoke to get the current version for each file (typically from
+# the version control system). Doxygen will invoke the program by executing (via
+# popen()) the command command input-file, where command is the value of the
+# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided
+# by doxygen. Whatever the program writes to standard output is used as the file
+# version. For an example see the documentation.
+
+FILE_VERSION_FILTER =
+
+# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed
+# by doxygen. The layout file controls the global structure of the generated
+# output files in an output format independent way. To create the layout file
+# that represents doxygen's defaults, run doxygen with the -l option. You can
+# optionally specify a file name after the option, if omitted DoxygenLayout.xml
+# will be used as the name of the layout file.
+#
+# Note that if you run doxygen from a directory containing a file called
+# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE
+# tag is left empty.
+
+LAYOUT_FILE = DoxygenLayout.xml
+
+# The CITE_BIB_FILES tag can be used to specify one or more bib files containing
+# the reference definitions. This must be a list of .bib files. The .bib
+# extension is automatically appended if omitted. This requires the bibtex tool
+# to be installed. See also http://en.wikipedia.org/wiki/BibTeX for more info.
+# For LaTeX the style of the bibliography can be controlled using
+# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the
+# search path. Do not use file names with spaces, bibtex cannot handle them. See
+# also \cite for info how to create references.
+
+CITE_BIB_FILES =
+
+#---------------------------------------------------------------------------
+# Configuration options related to warning and progress messages
+#---------------------------------------------------------------------------
+
+# The QUIET tag can be used to turn on/off the messages that are generated to
+# standard output by doxygen. If QUIET is set to YES this implies that the
+# messages are off.
+# The default value is: NO.
+
+QUIET = NO
+
+# The WARNINGS tag can be used to turn on/off the warning messages that are
+# generated to standard error ( stderr) by doxygen. If WARNINGS is set to YES
+# this implies that the warnings are on.
+#
+# Tip: Turn warnings on while writing the documentation.
+# The default value is: YES.
+
+WARNINGS = YES
+
+# If the WARN_IF_UNDOCUMENTED tag is set to YES, then doxygen will generate
+# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag
+# will automatically be disabled.
+# The default value is: YES.
+
+WARN_IF_UNDOCUMENTED = YES
+
+# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for
+# potential errors in the documentation, such as not documenting some parameters
+# in a documented function, or documenting parameters that don't exist or using
+# markup commands wrongly.
+# The default value is: YES.
+
+WARN_IF_DOC_ERROR = YES
+
+# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that
+# are documented, but have no documentation for their parameters or return
+# value. If set to NO doxygen will only warn about wrong or incomplete parameter
+# documentation, but not about the absence of documentation.
+# The default value is: NO.
+
+WARN_NO_PARAMDOC = NO
+
+# The WARN_FORMAT tag determines the format of the warning messages that doxygen
+# can produce. The string should contain the $file, $line, and $text tags, which
+# will be replaced by the file and line number from which the warning originated
+# and the warning text. Optionally the format may contain $version, which will
+# be replaced by the version of the file (if it could be obtained via
+# FILE_VERSION_FILTER)
+# The default value is: $file:$line: $text.
+
+WARN_FORMAT = "$file:$line: $text"
+
+# The WARN_LOGFILE tag can be used to specify a file to which warning and error
+# messages should be written. If left blank the output is written to standard
+# error (stderr).
+
+WARN_LOGFILE =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the input files
+#---------------------------------------------------------------------------
+
+# The INPUT tag is used to specify the files and/or directories that contain
+# documented source files. You may enter file names like myfile.cpp or
+# directories like /usr/src/myproject. Separate the files or directories with
+# spaces.
+# Note: If this tag is empty the current directory is searched.
+
+INPUT = ../ ./
+
+# This tag can be used to specify the character encoding of the source files
+# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses
+# libiconv (or the iconv built into libc) for the transcoding. See the libiconv
+# documentation (see: http://www.gnu.org/software/libiconv) for the list of
+# possible encodings.
+# The default value is: UTF-8.
+
+INPUT_ENCODING = UTF-8
+
+# If the value of the INPUT tag contains directories, you can use the
+# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and
+# *.h) to filter out the source-files in the directories. If left blank the
+# following patterns are tested:*.c, *.cc, *.cxx, *.cpp, *.c++, *.java, *.ii,
+# *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h, *.hh, *.hxx, *.hpp,
+# *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc, *.m, *.markdown,
+# *.md, *.mm, *.dox, *.py, *.f90, *.f, *.for, *.tcl, *.vhd, *.vhdl, *.ucf,
+# *.qsf, *.as and *.js.
+
+FILE_PATTERNS =
+
+# The RECURSIVE tag can be used to specify whether or not subdirectories should
+# be searched for input files as well.
+# The default value is: NO.
+
+RECURSIVE = YES
+
+# The EXCLUDE tag can be used to specify files and/or directories that should be
+# excluded from the INPUT source files. This way you can easily exclude a
+# subdirectory from a directory tree whose root is specified with the INPUT tag.
+#
+# Note that relative paths are relative to the directory from which doxygen is
+# run.
+
+EXCLUDE =
+
+# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or
+# directories that are symbolic links (a Unix file system feature) are excluded
+# from the input.
+# The default value is: NO.
+
+EXCLUDE_SYMLINKS = NO
+
+# If the value of the INPUT tag contains directories, you can use the
+# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude
+# certain files from those directories.
+#
+# Note that the wildcards are matched against the file with absolute path, so to
+# exclude all test directories for example use the pattern */test/*
+
+EXCLUDE_PATTERNS = */external/cppad/* \
+ */src/codegen/* \
+ */generated/jacobians* \
+ */generated/transforms*
+
+# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names
+# (namespaces, classes, functions, etc.) that should be excluded from the
+# output. The symbol name can be a fully qualified name, a word, or if the
+# wildcard * is used, a substring. Examples: ANamespace, AClass,
+# AClass::ANamespace, ANamespace::*Test
+#
+# Note that the wildcards are matched against the file with absolute path, so to
+# exclude all test directories use the pattern */test/*
+
+EXCLUDE_SYMBOLS =
+
+# The EXAMPLE_PATH tag can be used to specify one or more files or directories
+# that contain example code fragments that are included (see the \include
+# command).
+
+EXAMPLE_PATH = ../test
+
+# If the value of the EXAMPLE_PATH tag contains directories, you can use the
+# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and
+# *.h) to filter out the source-files in the directories. If left blank all
+# files are included.
+
+EXAMPLE_PATTERNS = *.cpp
+
+# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be
+# searched for input files to be used with the \include or \dontinclude commands
+# irrespective of the value of the RECURSIVE tag.
+# The default value is: NO.
+
+EXAMPLE_RECURSIVE = YES
+
+# The IMAGE_PATH tag can be used to specify one or more files or directories
+# that contain images that are to be included in the documentation (see the
+# \image command).
+
+IMAGE_PATH =
+
+# The INPUT_FILTER tag can be used to specify a program that doxygen should
+# invoke to filter for each input file. Doxygen will invoke the filter program
+# by executing (via popen()) the command:
+#
+# <filter> <input-file>
+#
+# where <filter> is the value of the INPUT_FILTER tag, and <input-file> is the
+# name of an input file. Doxygen will then use the output that the filter
+# program writes to standard output. If FILTER_PATTERNS is specified, this tag
+# will be ignored.
+#
+# Note that the filter must not add or remove lines; it is applied before the
+# code is scanned, but not when the output code is generated. If lines are added
+# or removed, the anchors will not be placed correctly.
+
+INPUT_FILTER =
+
+# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern
+# basis. Doxygen will compare the file name with each pattern and apply the
+# filter if there is a match. The filters are a list of the form: pattern=filter
+# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how
+# filters are used. If the FILTER_PATTERNS tag is empty or if none of the
+# patterns match the file name, INPUT_FILTER is applied.
+
+FILTER_PATTERNS =
+
+# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using
+# INPUT_FILTER ) will also be used to filter the input files that are used for
+# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES).
+# The default value is: NO.
+
+FILTER_SOURCE_FILES = NO
+
+# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file
+# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and
+# it is also possible to disable source filtering for a specific pattern using
+# *.ext= (so without naming a filter).
+# This tag requires that the tag FILTER_SOURCE_FILES is set to YES.
+
+FILTER_SOURCE_PATTERNS =
+
+# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that
+# is part of the input, its contents will be placed on the main page
+# (index.html). This can be useful if you have a project on for instance GitHub
+# and want to reuse the introduction page also for the doxygen output.
+
+USE_MDFILE_AS_MAINPAGE =
+
+#---------------------------------------------------------------------------
+# Configuration options related to source browsing
+#---------------------------------------------------------------------------
+
+# If the SOURCE_BROWSER tag is set to YES then a list of source files will be
+# generated. Documented entities will be cross-referenced with these sources.
+#
+# Note: To get rid of all source code in the generated output, make sure that
+# also VERBATIM_HEADERS is set to NO.
+# The default value is: NO.
+
+SOURCE_BROWSER = NO
+
+# Setting the INLINE_SOURCES tag to YES will include the body of functions,
+# classes and enums directly into the documentation.
+# The default value is: NO.
+
+INLINE_SOURCES = NO
+
+# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any
+# special comment blocks from generated source code fragments. Normal C, C++ and
+# Fortran comments will always remain visible.
+# The default value is: YES.
+
+STRIP_CODE_COMMENTS = YES
+
+# If the REFERENCED_BY_RELATION tag is set to YES then for each documented
+# function all documented functions referencing it will be listed.
+# The default value is: NO.
+
+REFERENCED_BY_RELATION = YES
+
+# If the REFERENCES_RELATION tag is set to YES then for each documented function
+# all documented entities called/used by that function will be listed.
+# The default value is: NO.
+
+REFERENCES_RELATION = YES
+
+# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set
+# to YES, then the hyperlinks from functions in REFERENCES_RELATION and
+# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will
+# link to the documentation.
+# The default value is: YES.
+
+REFERENCES_LINK_SOURCE = YES
+
+# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the
+# source code will show a tooltip with additional information such as prototype,
+# brief description and links to the definition and documentation. Since this
+# will make the HTML file larger and loading of large files a bit slower, you
+# can opt to disable this feature.
+# The default value is: YES.
+# This tag requires that the tag SOURCE_BROWSER is set to YES.
+
+SOURCE_TOOLTIPS = YES
+
+# If the USE_HTAGS tag is set to YES then the references to source code will
+# point to the HTML generated by the htags(1) tool instead of doxygen built-in
+# source browser. The htags tool is part of GNU's global source tagging system
+# (see http://www.gnu.org/software/global/global.html). You will need version
+# 4.8.6 or higher.
+#
+# To use it do the following:
+# - Install the latest version of global
+# - Enable SOURCE_BROWSER and USE_HTAGS in the config file
+# - Make sure the INPUT points to the root of the source tree
+# - Run doxygen as normal
+#
+# Doxygen will invoke htags (and that will in turn invoke gtags), so these
+# tools must be available from the command line (i.e. in the search path).
+#
+# The result: instead of the source browser generated by doxygen, the links to
+# source code will now point to the output of htags.
+# The default value is: NO.
+# This tag requires that the tag SOURCE_BROWSER is set to YES.
+
+USE_HTAGS = NO
+
+# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a
+# verbatim copy of the header file for each class for which an include is
+# specified. Set to NO to disable this.
+# See also: Section \class.
+# The default value is: YES.
+
+VERBATIM_HEADERS = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to the alphabetical class index
+#---------------------------------------------------------------------------
+
+# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all
+# compounds will be generated. Enable this if the project contains a lot of
+# classes, structs, unions or interfaces.
+# The default value is: YES.
+
+ALPHABETICAL_INDEX = YES
+
+# The COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns in
+# which the alphabetical index list will be split.
+# Minimum value: 1, maximum value: 20, default value: 5.
+# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
+
+COLS_IN_ALPHA_INDEX = 5
+
+# In case all classes in a project start with a common prefix, all classes will
+# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag
+# can be used to specify a prefix (or a list of prefixes) that should be ignored
+# while generating the index headers.
+# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
+
+IGNORE_PREFIX =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the HTML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_HTML tag is set to YES doxygen will generate HTML output
+# The default value is: YES.
+
+GENERATE_HTML = YES
+
+# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: html.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_OUTPUT = html
+
+# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each
+# generated HTML page (for example: .htm, .php, .asp).
+# The default value is: .html.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FILE_EXTENSION = .html
+
+# The HTML_HEADER tag can be used to specify a user-defined HTML header file for
+# each generated HTML page. If the tag is left blank doxygen will generate a
+# standard header.
+#
+# To get valid HTML the header file that includes any scripts and style sheets
+# that doxygen needs, which is dependent on the configuration options used (e.g.
+# the setting GENERATE_TREEVIEW). It is highly recommended to start with a
+# default header using
+# doxygen -w html new_header.html new_footer.html new_stylesheet.css
+# YourConfigFile
+# and then modify the file new_header.html. See also section "Doxygen usage"
+# for information on how to generate the default header that doxygen normally
+# uses.
+# Note: The header is subject to change so you typically have to regenerate the
+# default header when upgrading to a newer version of doxygen. For a description
+# of the possible markers and block names see the documentation.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_HEADER =
+
+# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each
+# generated HTML page. If the tag is left blank doxygen will generate a standard
+# footer. See HTML_HEADER for more information on how to generate a default
+# footer and what special commands can be used inside the footer. See also
+# section "Doxygen usage" for information on how to generate the default footer
+# that doxygen normally uses.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FOOTER =
+
+# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style
+# sheet that is used by each HTML page. It can be used to fine-tune the look of
+# the HTML output. If left blank doxygen will generate a default style sheet.
+# See also section "Doxygen usage" for information on how to generate the style
+# sheet that doxygen normally uses.
+# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as
+# it is more robust and this tag (HTML_STYLESHEET) will in the future become
+# obsolete.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_STYLESHEET =
+
+# The HTML_EXTRA_STYLESHEET tag can be used to specify an additional user-
+# defined cascading style sheet that is included after the standard style sheets
+# created by doxygen. Using this option one can overrule certain style aspects.
+# This is preferred over using HTML_STYLESHEET since it does not replace the
+# standard style sheet and is therefor more robust against future updates.
+# Doxygen will copy the style sheet file to the output directory. For an example
+# see the documentation.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_EXTRA_STYLESHEET =
+
+# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or
+# other source files which should be copied to the HTML output directory. Note
+# that these files will be copied to the base HTML output directory. Use the
+# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these
+# files. In the HTML_STYLESHEET file, use the file name only. Also note that the
+# files will be copied as-is; there are no commands or markers available.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_EXTRA_FILES =
+
+# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen
+# will adjust the colors in the stylesheet and background images according to
+# this color. Hue is specified as an angle on a colorwheel, see
+# http://en.wikipedia.org/wiki/Hue for more information. For instance the value
+# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300
+# purple, and 360 is red again.
+# Minimum value: 0, maximum value: 359, default value: 220.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_HUE = 290
+
+# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors
+# in the HTML output. For a value of 0 the output will use grayscales only. A
+# value of 255 will produce the most vivid colors.
+# Minimum value: 0, maximum value: 255, default value: 100.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_SAT = 100
+
+# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the
+# luminance component of the colors in the HTML output. Values below 100
+# gradually make the output lighter, whereas values above 100 make the output
+# darker. The value divided by 100 is the actual gamma applied, so 80 represents
+# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not
+# change the gamma.
+# Minimum value: 40, maximum value: 240, default value: 80.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_GAMMA = 80
+
+# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML
+# page will contain the date and time when the page was generated. Setting this
+# to NO can help when comparing the output of multiple runs.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_TIMESTAMP = YES
+
+# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML
+# documentation will contain sections that can be hidden and shown after the
+# page has loaded.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_DYNAMIC_SECTIONS = NO
+
+# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries
+# shown in the various tree structured indices initially; the user can expand
+# and collapse entries dynamically later on. Doxygen will expand the tree to
+# such a level that at most the specified number of entries are visible (unless
+# a fully collapsed tree already exceeds this amount). So setting the number of
+# entries 1 will produce a full collapsed tree by default. 0 is a special value
+# representing an infinite number of entries and will result in a full expanded
+# tree by default.
+# Minimum value: 0, maximum value: 9999, default value: 100.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_INDEX_NUM_ENTRIES = 100
+
+# If the GENERATE_DOCSET tag is set to YES, additional index files will be
+# generated that can be used as input for Apple's Xcode 3 integrated development
+# environment (see: http://developer.apple.com/tools/xcode/), introduced with
+# OSX 10.5 (Leopard). To create a documentation set, doxygen will generate a
+# Makefile in the HTML output directory. Running make will produce the docset in
+# that directory and running make install will install the docset in
+# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at
+# startup. See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html
+# for more information.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_DOCSET = NO
+
+# This tag determines the name of the docset feed. A documentation feed provides
+# an umbrella under which multiple documentation sets from a single provider
+# (such as a company or product suite) can be grouped.
+# The default value is: Doxygen generated docs.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_FEEDNAME = "Doxygen generated docs"
+
+# This tag specifies a string that should uniquely identify the documentation
+# set bundle. This should be a reverse domain-name style string, e.g.
+# com.mycompany.MyDocSet. Doxygen will append .docset to the name.
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_BUNDLE_ID = org.doxygen.Project
+
+# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify
+# the documentation publisher. This should be a reverse domain-name style
+# string, e.g. com.mycompany.MyDocSet.documentation.
+# The default value is: org.doxygen.Publisher.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_PUBLISHER_ID = org.doxygen.Publisher
+
+# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher.
+# The default value is: Publisher.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_PUBLISHER_NAME = Publisher
+
+# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three
+# additional HTML index files: index.hhp, index.hhc, and index.hhk. The
+# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop
+# (see: http://www.microsoft.com/en-us/download/details.aspx?id=21138) on
+# Windows.
+#
+# The HTML Help Workshop contains a compiler that can convert all HTML output
+# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML
+# files are now used as the Windows 98 help format, and will replace the old
+# Windows help format (.hlp) on all Windows platforms in the future. Compressed
+# HTML files also contain an index, a table of contents, and you can search for
+# words in the documentation. The HTML workshop also contains a viewer for
+# compressed HTML files.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_HTMLHELP = NO
+
+# The CHM_FILE tag can be used to specify the file name of the resulting .chm
+# file. You can add a path in front of the file if the result should not be
+# written to the html output directory.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+CHM_FILE =
+
+# The HHC_LOCATION tag can be used to specify the location (absolute path
+# including file name) of the HTML help compiler ( hhc.exe). If non-empty
+# doxygen will try to run the HTML help compiler on the generated index.hhp.
+# The file has to be specified with full path.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+HHC_LOCATION =
+
+# The GENERATE_CHI flag controls if a separate .chi index file is generated (
+# YES) or that it should be included in the master .chm file ( NO).
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+GENERATE_CHI = NO
+
+# The CHM_INDEX_ENCODING is used to encode HtmlHelp index ( hhk), content ( hhc)
+# and project file content.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+CHM_INDEX_ENCODING =
+
+# The BINARY_TOC flag controls whether a binary table of contents is generated (
+# YES) or a normal table of contents ( NO) in the .chm file. Furthermore it
+# enables the Previous and Next buttons.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+BINARY_TOC = NO
+
+# The TOC_EXPAND flag can be set to YES to add extra items for group members to
+# the table of contents of the HTML help documentation and to the tree view.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+TOC_EXPAND = NO
+
+# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and
+# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that
+# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help
+# (.qch) of the generated HTML documentation.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_QHP = NO
+
+# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify
+# the file name of the resulting .qch file. The path specified is relative to
+# the HTML output folder.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QCH_FILE =
+
+# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help
+# Project output. For more information please see Qt Help Project / Namespace
+# (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#namespace).
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_NAMESPACE = org.doxygen.Project
+
+# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt
+# Help Project output. For more information please see Qt Help Project / Virtual
+# Folders (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#virtual-
+# folders).
+# The default value is: doc.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_VIRTUAL_FOLDER = doc
+
+# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom
+# filter to add. For more information please see Qt Help Project / Custom
+# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
+# filters).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_CUST_FILTER_NAME =
+
+# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the
+# custom filter to add. For more information please see Qt Help Project / Custom
+# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
+# filters).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_CUST_FILTER_ATTRS =
+
+# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this
+# project's filter section matches. Qt Help Project / Filter Attributes (see:
+# http://qt-project.org/doc/qt-4.8/qthelpproject.html#filter-attributes).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_SECT_FILTER_ATTRS =
+
+# The QHG_LOCATION tag can be used to specify the location of Qt's
+# qhelpgenerator. If non-empty doxygen will try to run qhelpgenerator on the
+# generated .qhp file.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHG_LOCATION =
+
+# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be
+# generated, together with the HTML files, they form an Eclipse help plugin. To
+# install this plugin and make it available under the help contents menu in
+# Eclipse, the contents of the directory containing the HTML and XML files needs
+# to be copied into the plugins directory of eclipse. The name of the directory
+# within the plugins directory should be the same as the ECLIPSE_DOC_ID value.
+# After copying Eclipse needs to be restarted before the help appears.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_ECLIPSEHELP = NO
+
+# A unique identifier for the Eclipse help plugin. When installing the plugin
+# the directory name containing the HTML and XML files should also have this
+# name. Each documentation set should have its own identifier.
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES.
+
+ECLIPSE_DOC_ID = org.doxygen.Project
+
+# If you want full control over the layout of the generated HTML pages it might
+# be necessary to disable the index and replace it with your own. The
+# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top
+# of each HTML page. A value of NO enables the index and the value YES disables
+# it. Since the tabs in the index contain the same information as the navigation
+# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+DISABLE_INDEX = NO
+
+# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index
+# structure should be generated to display hierarchical information. If the tag
+# value is set to YES, a side panel will be generated containing a tree-like
+# index structure (just like the one that is generated for HTML Help). For this
+# to work a browser that supports JavaScript, DHTML, CSS and frames is required
+# (i.e. any modern browser). Windows users are probably better off using the
+# HTML help feature. Via custom stylesheets (see HTML_EXTRA_STYLESHEET) one can
+# further fine-tune the look of the index. As an example, the default style
+# sheet generated by doxygen has an example that shows how to put an image at
+# the root of the tree instead of the PROJECT_NAME. Since the tree basically has
+# the same information as the tab index, you could consider setting
+# DISABLE_INDEX to YES when enabling this option.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_TREEVIEW = YES
+
+# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that
+# doxygen will group on one line in the generated HTML documentation.
+#
+# Note that a value of 0 will completely suppress the enum values from appearing
+# in the overview section.
+# Minimum value: 0, maximum value: 20, default value: 4.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+ENUM_VALUES_PER_LINE = 4
+
+# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used
+# to set the initial width (in pixels) of the frame in which the tree is shown.
+# Minimum value: 0, maximum value: 1500, default value: 250.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+TREEVIEW_WIDTH = 250
+
+# When the EXT_LINKS_IN_WINDOW option is set to YES doxygen will open links to
+# external symbols imported via tag files in a separate window.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+EXT_LINKS_IN_WINDOW = NO
+
+# Use this tag to change the font size of LaTeX formulas included as images in
+# the HTML documentation. When you change the font size after a successful
+# doxygen run you need to manually remove any form_*.png images from the HTML
+# output directory to force them to be regenerated.
+# Minimum value: 8, maximum value: 50, default value: 10.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+FORMULA_FONTSIZE = 10
+
+# Use the FORMULA_TRANPARENT tag to determine whether or not the images
+# generated for formulas are transparent PNGs. Transparent PNGs are not
+# supported properly for IE 6.0, but are supported on all modern browsers.
+#
+# Note that when changing this option you need to delete any form_*.png files in
+# the HTML output directory before the changes have effect.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+FORMULA_TRANSPARENT = YES
+
+# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see
+# http://www.mathjax.org) which uses client side Javascript for the rendering
+# instead of using prerendered bitmaps. Use this if you do not have LaTeX
+# installed or if you want to formulas look prettier in the HTML output. When
+# enabled you may also need to install MathJax separately and configure the path
+# to it using the MATHJAX_RELPATH option.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+USE_MATHJAX = NO
+
+# When MathJax is enabled you can set the default output format to be used for
+# the MathJax output. See the MathJax site (see:
+# http://docs.mathjax.org/en/latest/output.html) for more details.
+# Possible values are: HTML-CSS (which is slower, but has the best
+# compatibility), NativeMML (i.e. MathML) and SVG.
+# The default value is: HTML-CSS.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_FORMAT = HTML-CSS
+
+# When MathJax is enabled you need to specify the location relative to the HTML
+# output directory using the MATHJAX_RELPATH option. The destination directory
+# should contain the MathJax.js script. For instance, if the mathjax directory
+# is located at the same level as the HTML output directory, then
+# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax
+# Content Delivery Network so you can quickly see the result without installing
+# MathJax. However, it is strongly recommended to install a local copy of
+# MathJax from http://www.mathjax.org before deployment.
+# The default value is: http://cdn.mathjax.org/mathjax/latest.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest
+
+# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax
+# extension names that should be enabled during MathJax rendering. For example
+# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_EXTENSIONS =
+
+# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces
+# of code that will be used on startup of the MathJax code. See the MathJax site
+# (see: http://docs.mathjax.org/en/latest/output.html) for more details. For an
+# example see the documentation.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_CODEFILE =
+
+# When the SEARCHENGINE tag is enabled doxygen will generate a search box for
+# the HTML output. The underlying search engine uses javascript and DHTML and
+# should work on any modern browser. Note that when using HTML help
+# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET)
+# there is already a search function so this one should typically be disabled.
+# For large projects the javascript based search engine can be slow, then
+# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to
+# search using the keyboard; to jump to the search box use <access key> + S
+# (what the <access key> is depends on the OS and browser, but it is typically
+# <CTRL>, <ALT>/<option>, or both). Inside the search box use the <cursor down
+# key> to jump into the search results window, the results can be navigated
+# using the <cursor keys>. Press <Enter> to select an item or <escape> to cancel
+# the search. The filter options can be selected when the cursor is inside the
+# search box by pressing <Shift>+<cursor down>. Also here use the <cursor keys>
+# to select a filter and <Enter> or <escape> to activate or cancel the filter
+# option.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+SEARCHENGINE = YES
+
+# When the SERVER_BASED_SEARCH tag is enabled the search engine will be
+# implemented using a web server instead of a web client using Javascript. There
+# are two flavors of web server based searching depending on the EXTERNAL_SEARCH
+# setting. When disabled, doxygen will generate a PHP script for searching and
+# an index file used by the script. When EXTERNAL_SEARCH is enabled the indexing
+# and searching needs to be provided by external tools. See the section
+# "External Indexing and Searching" for details.
+# The default value is: NO.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SERVER_BASED_SEARCH = NO
+
+# When EXTERNAL_SEARCH tag is enabled doxygen will no longer generate the PHP
+# script for searching. Instead the search results are written to an XML file
+# which needs to be processed by an external indexer. Doxygen will invoke an
+# external search engine pointed to by the SEARCHENGINE_URL option to obtain the
+# search results.
+#
+# Doxygen ships with an example indexer ( doxyindexer) and search engine
+# (doxysearch.cgi) which are based on the open source search engine library
+# Xapian (see: http://xapian.org/).
+#
+# See the section "External Indexing and Searching" for details.
+# The default value is: NO.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTERNAL_SEARCH = NO
+
+# The SEARCHENGINE_URL should point to a search engine hosted by a web server
+# which will return the search results when EXTERNAL_SEARCH is enabled.
+#
+# Doxygen ships with an example indexer ( doxyindexer) and search engine
+# (doxysearch.cgi) which are based on the open source search engine library
+# Xapian (see: http://xapian.org/). See the section "External Indexing and
+# Searching" for details.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SEARCHENGINE_URL =
+
+# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the unindexed
+# search data is written to a file for indexing by an external tool. With the
+# SEARCHDATA_FILE tag the name of this file can be specified.
+# The default file is: searchdata.xml.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SEARCHDATA_FILE = searchdata.xml
+
+# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the
+# EXTERNAL_SEARCH_ID tag can be used as an identifier for the project. This is
+# useful in combination with EXTRA_SEARCH_MAPPINGS to search through multiple
+# projects and redirect the results back to the right project.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTERNAL_SEARCH_ID =
+
+# The EXTRA_SEARCH_MAPPINGS tag can be used to enable searching through doxygen
+# projects other than the one defined by this configuration file, but that are
+# all added to the same external search index. Each project needs to have a
+# unique id set via EXTERNAL_SEARCH_ID. The search mapping then maps the id of
+# to a relative location where the documentation can be found. The format is:
+# EXTRA_SEARCH_MAPPINGS = tagname1=loc1 tagname2=loc2 ...
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTRA_SEARCH_MAPPINGS =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the LaTeX output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_LATEX tag is set to YES doxygen will generate LaTeX output.
+# The default value is: YES.
+
+GENERATE_LATEX = NO
+
+# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: latex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_OUTPUT = latex
+
+# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be
+# invoked.
+#
+# Note that when enabling USE_PDFLATEX this option is only used for generating
+# bitmaps for formulas in the HTML output, but not in the Makefile that is
+# written to the output directory.
+# The default file is: latex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_CMD_NAME = latex
+
+# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to generate
+# index for LaTeX.
+# The default file is: makeindex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+MAKEINDEX_CMD_NAME = makeindex
+
+# If the COMPACT_LATEX tag is set to YES doxygen generates more compact LaTeX
+# documents. This may be useful for small projects and may help to save some
+# trees in general.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+COMPACT_LATEX = NO
+
+# The PAPER_TYPE tag can be used to set the paper type that is used by the
+# printer.
+# Possible values are: a4 (210 x 297 mm), letter (8.5 x 11 inches), legal (8.5 x
+# 14 inches) and executive (7.25 x 10.5 inches).
+# The default value is: a4.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+PAPER_TYPE = a4
+
+# The EXTRA_PACKAGES tag can be used to specify one or more LaTeX package names
+# that should be included in the LaTeX output. To get the times font for
+# instance you can specify
+# EXTRA_PACKAGES=times
+# If left blank no extra packages will be included.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+EXTRA_PACKAGES = amsmath
+
+# The LATEX_HEADER tag can be used to specify a personal LaTeX header for the
+# generated LaTeX document. The header should contain everything until the first
+# chapter. If it is left blank doxygen will generate a standard header. See
+# section "Doxygen usage" for information on how to let doxygen write the
+# default header to a separate file.
+#
+# Note: Only use a user-defined header if you know what you are doing! The
+# following commands have a special meaning inside the header: $title,
+# $datetime, $date, $doxygenversion, $projectname, $projectnumber. Doxygen will
+# replace them by respectively the title of the page, the current date and time,
+# only the current date, the version number of doxygen, the project name (see
+# PROJECT_NAME), or the project number (see PROJECT_NUMBER).
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_HEADER =
+
+# The LATEX_FOOTER tag can be used to specify a personal LaTeX footer for the
+# generated LaTeX document. The footer should contain everything after the last
+# chapter. If it is left blank doxygen will generate a standard footer.
+#
+# Note: Only use a user-defined footer if you know what you are doing!
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_FOOTER =
+
+# The LATEX_EXTRA_FILES tag can be used to specify one or more extra images or
+# other source files which should be copied to the LATEX_OUTPUT output
+# directory. Note that the files will be copied as-is; there are no commands or
+# markers available.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_EXTRA_FILES =
+
+# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated is
+# prepared for conversion to PDF (using ps2pdf or pdflatex). The PDF file will
+# contain links (just like the HTML output) instead of page references. This
+# makes the output suitable for online browsing using a PDF viewer.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+PDF_HYPERLINKS = YES
+
+# If the LATEX_PDFLATEX tag is set to YES, doxygen will use pdflatex to generate
+# the PDF file directly from the LaTeX files. Set this option to YES to get a
+# higher quality PDF documentation.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+USE_PDFLATEX = YES
+
+# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \batchmode
+# command to the generated LaTeX files. This will instruct LaTeX to keep running
+# if errors occur, instead of asking the user for help. This option is also used
+# when generating formulas in HTML.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_BATCHMODE = NO
+
+# If the LATEX_HIDE_INDICES tag is set to YES then doxygen will not include the
+# index chapters (such as File Index, Compound Index, etc.) in the output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_HIDE_INDICES = NO
+
+# If the LATEX_SOURCE_CODE tag is set to YES then doxygen will include source
+# code with syntax highlighting in the LaTeX output.
+#
+# Note that which sources are shown also depends on other settings such as
+# SOURCE_BROWSER.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_SOURCE_CODE = NO
+
+# The LATEX_BIB_STYLE tag can be used to specify the style to use for the
+# bibliography, e.g. plainnat, or ieeetr. See
+# http://en.wikipedia.org/wiki/BibTeX and \cite for more info.
+# The default value is: plain.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_BIB_STYLE = plain
+
+#---------------------------------------------------------------------------
+# Configuration options related to the RTF output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_RTF tag is set to YES doxygen will generate RTF output. The
+# RTF output is optimized for Word 97 and may not look too pretty with other RTF
+# readers/editors.
+# The default value is: NO.
+
+GENERATE_RTF = NO
+
+# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: rtf.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_OUTPUT = rtf
+
+# If the COMPACT_RTF tag is set to YES doxygen generates more compact RTF
+# documents. This may be useful for small projects and may help to save some
+# trees in general.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+COMPACT_RTF = NO
+
+# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated will
+# contain hyperlink fields. The RTF file will contain links (just like the HTML
+# output) instead of page references. This makes the output suitable for online
+# browsing using Word or some other Word compatible readers that support those
+# fields.
+#
+# Note: WordPad (write) and others do not support links.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_HYPERLINKS = NO
+
+# Load stylesheet definitions from file. Syntax is similar to doxygen's config
+# file, i.e. a series of assignments. You only have to provide replacements,
+# missing definitions are set to their default value.
+#
+# See also section "Doxygen usage" for information on how to generate the
+# default style sheet that doxygen normally uses.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_STYLESHEET_FILE =
+
+# Set optional variables used in the generation of an RTF document. Syntax is
+# similar to doxygen's config file. A template extensions file can be generated
+# using doxygen -e rtf extensionFile.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_EXTENSIONS_FILE =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the man page output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_MAN tag is set to YES doxygen will generate man pages for
+# classes and files.
+# The default value is: NO.
+
+GENERATE_MAN = NO
+
+# The MAN_OUTPUT tag is used to specify where the man pages will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it. A directory man3 will be created inside the directory specified by
+# MAN_OUTPUT.
+# The default directory is: man.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_OUTPUT = man
+
+# The MAN_EXTENSION tag determines the extension that is added to the generated
+# man pages. In case the manual section does not start with a number, the number
+# 3 is prepended. The dot (.) at the beginning of the MAN_EXTENSION tag is
+# optional.
+# The default value is: .3.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_EXTENSION = .3
+
+# The MAN_SUBDIR tag determines the name of the directory created within
+# MAN_OUTPUT in which the man pages are placed. If defaults to man followed by
+# MAN_EXTENSION with the initial . removed.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_SUBDIR =
+
+# If the MAN_LINKS tag is set to YES and doxygen generates man output, then it
+# will generate one additional man file for each entity documented in the real
+# man page(s). These additional files only source the real man page, but without
+# them the man command would be unable to find the correct page.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_LINKS = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the XML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_XML tag is set to YES doxygen will generate an XML file that
+# captures the structure of the code including all documentation.
+# The default value is: NO.
+
+GENERATE_XML = NO
+
+# The XML_OUTPUT tag is used to specify where the XML pages will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: xml.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_OUTPUT = xml
+
+# If the XML_PROGRAMLISTING tag is set to YES doxygen will dump the program
+# listings (including syntax highlighting and cross-referencing information) to
+# the XML output. Note that enabling this will significantly increase the size
+# of the XML output.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_PROGRAMLISTING = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to the DOCBOOK output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_DOCBOOK tag is set to YES doxygen will generate Docbook files
+# that can be used to generate PDF.
+# The default value is: NO.
+
+GENERATE_DOCBOOK = NO
+
+# The DOCBOOK_OUTPUT tag is used to specify where the Docbook pages will be put.
+# If a relative path is entered the value of OUTPUT_DIRECTORY will be put in
+# front of it.
+# The default directory is: docbook.
+# This tag requires that the tag GENERATE_DOCBOOK is set to YES.
+
+DOCBOOK_OUTPUT = docbook
+
+#---------------------------------------------------------------------------
+# Configuration options for the AutoGen Definitions output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_AUTOGEN_DEF tag is set to YES doxygen will generate an AutoGen
+# Definitions (see http://autogen.sf.net) file that captures the structure of
+# the code including all documentation. Note that this feature is still
+# experimental and incomplete at the moment.
+# The default value is: NO.
+
+GENERATE_AUTOGEN_DEF = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the Perl module output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_PERLMOD tag is set to YES doxygen will generate a Perl module
+# file that captures the structure of the code including all documentation.
+#
+# Note that this feature is still experimental and incomplete at the moment.
+# The default value is: NO.
+
+GENERATE_PERLMOD = NO
+
+# If the PERLMOD_LATEX tag is set to YES doxygen will generate the necessary
+# Makefile rules, Perl scripts and LaTeX code to be able to generate PDF and DVI
+# output from the Perl module output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_LATEX = NO
+
+# If the PERLMOD_PRETTY tag is set to YES the Perl module output will be nicely
+# formatted so it can be parsed by a human reader. This is useful if you want to
+# understand what is going on. On the other hand, if this tag is set to NO the
+# size of the Perl module output will be much smaller and Perl will parse it
+# just the same.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_PRETTY = YES
+
+# The names of the make variables in the generated doxyrules.make file are
+# prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. This is useful
+# so different doxyrules.make files included by the same Makefile don't
+# overwrite each other's variables.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_MAKEVAR_PREFIX =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the preprocessor
+#---------------------------------------------------------------------------
+
+# If the ENABLE_PREPROCESSING tag is set to YES doxygen will evaluate all
+# C-preprocessor directives found in the sources and include files.
+# The default value is: YES.
+
+ENABLE_PREPROCESSING = YES
+
+# If the MACRO_EXPANSION tag is set to YES doxygen will expand all macro names
+# in the source code. If set to NO only conditional compilation will be
+# performed. Macro expansion can be done in a controlled way by setting
+# EXPAND_ONLY_PREDEF to YES.
+# The default value is: NO.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+MACRO_EXPANSION = NO
+
+# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES then
+# the macro expansion is limited to the macros specified with the PREDEFINED and
+# EXPAND_AS_DEFINED tags.
+# The default value is: NO.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+EXPAND_ONLY_PREDEF = NO
+
+# If the SEARCH_INCLUDES tag is set to YES the includes files in the
+# INCLUDE_PATH will be searched if a #include is found.
+# The default value is: YES.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+SEARCH_INCLUDES = YES
+
+# The INCLUDE_PATH tag can be used to specify one or more directories that
+# contain include files that are not input files but should be processed by the
+# preprocessor.
+# This tag requires that the tag SEARCH_INCLUDES is set to YES.
+
+INCLUDE_PATH =
+
+# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard
+# patterns (like *.h and *.hpp) to filter out the header-files in the
+# directories. If left blank, the patterns specified with FILE_PATTERNS will be
+# used.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+INCLUDE_FILE_PATTERNS =
+
+# The PREDEFINED tag can be used to specify one or more macro names that are
+# defined before the preprocessor is started (similar to the -D option of e.g.
+# gcc). The argument of the tag is a list of macros of the form: name or
+# name=definition (no spaces). If the definition and the "=" are omitted, "=1"
+# is assumed. To prevent a macro definition from being undefined via #undef or
+# recursively expanded use the := operator instead of the = operator.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+PREDEFINED = DOXYGEN_SHOULD_SKIP_THIS
+
+# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then this
+# tag can be used to specify a list of macro names that should be expanded. The
+# macro definition that is found in the sources will be used. Use the PREDEFINED
+# tag if you want to use a different macro definition that overrules the
+# definition found in the source code.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+EXPAND_AS_DEFINED =
+
+# If the SKIP_FUNCTION_MACROS tag is set to YES then doxygen's preprocessor will
+# remove all references to function-like macros that are alone on a line, have
+# an all uppercase name, and do not end with a semicolon. Such function macros
+# are typically used for boiler-plate code, and will confuse the parser if not
+# removed.
+# The default value is: YES.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+SKIP_FUNCTION_MACROS = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to external references
+#---------------------------------------------------------------------------
+
+# The TAGFILES tag can be used to specify one or more tag files. For each tag
+# file the location of the external documentation should be added. The format of
+# a tag file without this location is as follows:
+# TAGFILES = file1 file2 ...
+# Adding location for the tag files is done as follows:
+# TAGFILES = file1=loc1 "file2 = loc2" ...
+# where loc1 and loc2 can be relative or absolute paths or URLs. See the
+# section "Linking to external documentation" for more information about the use
+# of tag files.
+# Note: Each tag file must have a unique name (where the name does NOT include
+# the path). If a tag file is not located in the directory in which doxygen is
+# run, you must also specify the path to the tagfile here.
+
+TAGFILES = ../../ct_doc/doc/tags/ct_core.tag=../../../ct_core/doc/html \
+ ../../ct_doc/doc/tags/ct_optcon.tag=../../../ct_optcon/doc/html
+
+# When a file name is specified after GENERATE_TAGFILE, doxygen will create a
+# tag file that is based on the input files it reads. See section "Linking to
+# external documentation" for more information about the usage of tag files.
+
+GENERATE_TAGFILE = ../../ct_doc/doc/tags/ct_rbd.tag
+
+# If the ALLEXTERNALS tag is set to YES all external class will be listed in the
+# class index. If set to NO only the inherited external classes will be listed.
+# The default value is: NO.
+
+ALLEXTERNALS = NO
+
+# If the EXTERNAL_GROUPS tag is set to YES all external groups will be listed in
+# the modules index. If set to NO, only the current project's groups will be
+# listed.
+# The default value is: YES.
+
+EXTERNAL_GROUPS = NO
+
+# If the EXTERNAL_PAGES tag is set to YES all external pages will be listed in
+# the related pages index. If set to NO, only the current project's pages will
+# be listed.
+# The default value is: YES.
+
+EXTERNAL_PAGES = YES
+
+# The PERL_PATH should be the absolute path and name of the perl script
+# interpreter (i.e. the result of 'which perl').
+# The default file (with absolute path) is: /usr/bin/perl.
+
+PERL_PATH = /usr/bin/perl
+
+#---------------------------------------------------------------------------
+# Configuration options related to the dot tool
+#---------------------------------------------------------------------------
+
+# If the CLASS_DIAGRAMS tag is set to YES doxygen will generate a class diagram
+# (in HTML and LaTeX) for classes with base or super classes. Setting the tag to
+# NO turns the diagrams off. Note that this option also works with HAVE_DOT
+# disabled, but it is recommended to install and use dot, since it yields more
+# powerful graphs.
+# The default value is: YES.
+
+CLASS_DIAGRAMS = YES
+
+# You can define message sequence charts within doxygen comments using the \msc
+# command. Doxygen will then run the mscgen tool (see:
+# http://www.mcternan.me.uk/mscgen/)) to produce the chart and insert it in the
+# documentation. The MSCGEN_PATH tag allows you to specify the directory where
+# the mscgen tool resides. If left empty the tool is assumed to be found in the
+# default search path.
+
+MSCGEN_PATH =
+
+# You can include diagrams made with dia in doxygen documentation. Doxygen will
+# then run dia to produce the diagram and insert it in the documentation. The
+# DIA_PATH tag allows you to specify the directory where the dia binary resides.
+# If left empty dia is assumed to be found in the default search path.
+
+DIA_PATH =
+
+# If set to YES, the inheritance and collaboration graphs will hide inheritance
+# and usage relations if the target is undocumented or is not a class.
+# The default value is: YES.
+
+HIDE_UNDOC_RELATIONS = YES
+
+# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is
+# available from the path. This tool is part of Graphviz (see:
+# http://www.graphviz.org/), a graph visualization toolkit from AT&T and Lucent
+# Bell Labs. The other options in this section have no effect if this option is
+# set to NO
+# The default value is: YES.
+
+HAVE_DOT = NO
+
+# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is allowed
+# to run in parallel. When set to 0 doxygen will base this on the number of
+# processors available in the system. You can set it explicitly to a value
+# larger than 0 to get control over the balance between CPU load and processing
+# speed.
+# Minimum value: 0, maximum value: 32, default value: 0.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_NUM_THREADS = 0
+
+# When you want a differently looking font n the dot files that doxygen
+# generates you can specify the font name using DOT_FONTNAME. You need to make
+# sure dot is able to find the font, which can be done by putting it in a
+# standard location or by setting the DOTFONTPATH environment variable or by
+# setting DOT_FONTPATH to the directory containing the font.
+# The default value is: Helvetica.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTNAME = Helvetica
+
+# The DOT_FONTSIZE tag can be used to set the size (in points) of the font of
+# dot graphs.
+# Minimum value: 4, maximum value: 24, default value: 10.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTSIZE = 10
+
+# By default doxygen will tell dot to use the default font as specified with
+# DOT_FONTNAME. If you specify a different font using DOT_FONTNAME you can set
+# the path where dot can find it using this tag.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTPATH =
+
+# If the CLASS_GRAPH tag is set to YES then doxygen will generate a graph for
+# each documented class showing the direct and indirect inheritance relations.
+# Setting this tag to YES will force the CLASS_DIAGRAMS tag to NO.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CLASS_GRAPH = YES
+
+# If the COLLABORATION_GRAPH tag is set to YES then doxygen will generate a
+# graph for each documented class showing the direct and indirect implementation
+# dependencies (inheritance, containment, and class references variables) of the
+# class with other documented classes.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+COLLABORATION_GRAPH = YES
+
+# If the GROUP_GRAPHS tag is set to YES then doxygen will generate a graph for
+# groups, showing the direct groups dependencies.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GROUP_GRAPHS = YES
+
+# If the UML_LOOK tag is set to YES doxygen will generate inheritance and
+# collaboration diagrams in a style similar to the OMG's Unified Modeling
+# Language.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+UML_LOOK = NO
+
+# If the UML_LOOK tag is enabled, the fields and methods are shown inside the
+# class node. If there are many fields or methods and many nodes the graph may
+# become too big to be useful. The UML_LIMIT_NUM_FIELDS threshold limits the
+# number of items for each type to make the size more manageable. Set this to 0
+# for no limit. Note that the threshold may be exceeded by 50% before the limit
+# is enforced. So when you set the threshold to 10, up to 15 fields may appear,
+# but if the number exceeds 15, the total amount of fields shown is limited to
+# 10.
+# Minimum value: 0, maximum value: 100, default value: 10.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+UML_LIMIT_NUM_FIELDS = 10
+
+# If the TEMPLATE_RELATIONS tag is set to YES then the inheritance and
+# collaboration graphs will show the relations between templates and their
+# instances.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+TEMPLATE_RELATIONS = NO
+
+# If the INCLUDE_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are set to
+# YES then doxygen will generate a graph for each documented file showing the
+# direct and indirect include dependencies of the file with other documented
+# files.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INCLUDE_GRAPH = YES
+
+# If the INCLUDED_BY_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are
+# set to YES then doxygen will generate a graph for each documented file showing
+# the direct and indirect include dependencies of the file with other documented
+# files.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INCLUDED_BY_GRAPH = YES
+
+# If the CALL_GRAPH tag is set to YES then doxygen will generate a call
+# dependency graph for every global function or class method.
+#
+# Note that enabling this option will significantly increase the time of a run.
+# So in most cases it will be better to enable call graphs for selected
+# functions only using the \callgraph command.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CALL_GRAPH = NO
+
+# If the CALLER_GRAPH tag is set to YES then doxygen will generate a caller
+# dependency graph for every global function or class method.
+#
+# Note that enabling this option will significantly increase the time of a run.
+# So in most cases it will be better to enable caller graphs for selected
+# functions only using the \callergraph command.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CALLER_GRAPH = NO
+
+# If the GRAPHICAL_HIERARCHY tag is set to YES then doxygen will graphical
+# hierarchy of all classes instead of a textual one.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GRAPHICAL_HIERARCHY = YES
+
+# If the DIRECTORY_GRAPH tag is set to YES then doxygen will show the
+# dependencies a directory has on other directories in a graphical way. The
+# dependency relations are determined by the #include relations between the
+# files in the directories.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DIRECTORY_GRAPH = YES
+
+# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images
+# generated by dot.
+# Note: If you choose svg you need to set HTML_FILE_EXTENSION to xhtml in order
+# to make the SVG files visible in IE 9+ (other browsers do not have this
+# requirement).
+# Possible values are: png, png:cairo, png:cairo:cairo, png:cairo:gd, png:gd,
+# png:gd:gd, jpg, jpg:cairo, jpg:cairo:gd, jpg:gd, jpg:gd:gd, gif, gif:cairo,
+# gif:cairo:gd, gif:gd, gif:gd:gd and svg.
+# The default value is: png.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_IMAGE_FORMAT = png
+
+# If DOT_IMAGE_FORMAT is set to svg, then this option can be set to YES to
+# enable generation of interactive SVG images that allow zooming and panning.
+#
+# Note that this requires a modern browser other than Internet Explorer. Tested
+# and working are Firefox, Chrome, Safari, and Opera.
+# Note: For IE 9+ you need to set HTML_FILE_EXTENSION to xhtml in order to make
+# the SVG files visible. Older versions of IE do not have SVG support.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INTERACTIVE_SVG = NO
+
+# The DOT_PATH tag can be used to specify the path where the dot tool can be
+# found. If left blank, it is assumed the dot tool can be found in the path.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_PATH =
+
+# The DOTFILE_DIRS tag can be used to specify one or more directories that
+# contain dot files that are included in the documentation (see the \dotfile
+# command).
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOTFILE_DIRS =
+
+# The MSCFILE_DIRS tag can be used to specify one or more directories that
+# contain msc files that are included in the documentation (see the \mscfile
+# command).
+
+MSCFILE_DIRS =
+
+# The DIAFILE_DIRS tag can be used to specify one or more directories that
+# contain dia files that are included in the documentation (see the \diafile
+# command).
+
+DIAFILE_DIRS =
+
+# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of nodes
+# that will be shown in the graph. If the number of nodes in a graph becomes
+# larger than this value, doxygen will truncate the graph, which is visualized
+# by representing a node as a red box. Note that doxygen if the number of direct
+# children of the root node in a graph is already larger than
+# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note that
+# the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH.
+# Minimum value: 0, maximum value: 10000, default value: 50.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_GRAPH_MAX_NODES = 50
+
+# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the graphs
+# generated by dot. A depth value of 3 means that only nodes reachable from the
+# root by following a path via at most 3 edges will be shown. Nodes that lay
+# further from the root node will be omitted. Note that setting this option to 1
+# or 2 may greatly reduce the computation time needed for large code bases. Also
+# note that the size of a graph can be further restricted by
+# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction.
+# Minimum value: 0, maximum value: 1000, default value: 0.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+MAX_DOT_GRAPH_DEPTH = 0
+
+# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent
+# background. This is disabled by default, because dot on Windows does not seem
+# to support this out of the box.
+#
+# Warning: Depending on the platform used, enabling this option may lead to
+# badly anti-aliased labels on the edges of a graph (i.e. they become hard to
+# read).
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_TRANSPARENT = NO
+
+# Set the DOT_MULTI_TARGETS tag to YES allow dot to generate multiple output
+# files in one run (i.e. multiple -o and -T options on the command line). This
+# makes dot run faster, but since only newer versions of dot (>1.8.10) support
+# this, this feature is disabled by default.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_MULTI_TARGETS = YES
+
+# If the GENERATE_LEGEND tag is set to YES doxygen will generate a legend page
+# explaining the meaning of the various boxes and arrows in the dot generated
+# graphs.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GENERATE_LEGEND = YES
+
+# If the DOT_CLEANUP tag is set to YES doxygen will remove the intermediate dot
+# files that are used to generate the various graphs.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_CLEANUP = YES
diff --git a/ct_rbd/doc/DoxygenLayout.xml b/ct_rbd/doc/DoxygenLayout.xml
new file mode 100644
index 0000000..e67ce49
--- /dev/null
+++ b/ct_rbd/doc/DoxygenLayout.xml
@@ -0,0 +1,194 @@
+<doxygenlayout version="1.0">
+ <!-- Generated by doxygen 1.8.7 -->
+ <!-- Navigation index tabs for HTML output -->
+ <navindex>
+ <tab type="mainpage" visible="yes" title=""/>
+ <tab type="pages" visible="yes" title="" intro=""/>
+ <tab type="modules" visible="no" title="" intro=""/>
+ <tab type="namespaces" visible="yes" title="">
+ <tab type="namespacelist" visible="yes" title="" intro=""/>
+ <tab type="namespacemembers" visible="yes" title="" intro=""/>
+ </tab>
+ <tab type="classes" visible="yes" title="">
+ <tab type="classlist" visible="yes" title="" intro=""/>
+ <tab type="classindex" visible="$ALPHABETICAL_INDEX" title=""/>
+ <tab type="hierarchy" visible="yes" title="" intro=""/>
+ <tab type="classmembers" visible="yes" title="" intro=""/>
+ </tab>
+ <tab type="files" visible="yes" title="">
+ <tab type="filelist" visible="yes" title="" intro=""/>
+ <tab type="globals" visible="yes" title="" intro=""/>
+ </tab>
+ <tab type="examples" visible="yes" title="" intro=""/>
+ </navindex>
+
+ <!-- Layout definition for a class page -->
+ <class>
+ <briefdescription visible="yes"/>
+ <includes visible="$SHOW_INCLUDE_FILES"/>
+ <inheritancegraph visible="$CLASS_GRAPH"/>
+ <collaborationgraph visible="$COLLABORATION_GRAPH"/>
+ <memberdecl>
+ <nestedclasses visible="yes" title=""/>
+ <publictypes title=""/>
+ <services title=""/>
+ <interfaces title=""/>
+ <publicslots title=""/>
+ <signals title=""/>
+ <publicmethods title=""/>
+ <publicstaticmethods title=""/>
+ <publicattributes title=""/>
+ <publicstaticattributes title=""/>
+ <protectedtypes title=""/>
+ <protectedslots title=""/>
+ <protectedmethods title=""/>
+ <protectedstaticmethods title=""/>
+ <protectedattributes title=""/>
+ <protectedstaticattributes title=""/>
+ <packagetypes title=""/>
+ <packagemethods title=""/>
+ <packagestaticmethods title=""/>
+ <packageattributes title=""/>
+ <packagestaticattributes title=""/>
+ <properties title=""/>
+ <events title=""/>
+ <privatetypes title=""/>
+ <privateslots title=""/>
+ <privatemethods title=""/>
+ <privatestaticmethods title=""/>
+ <privateattributes title=""/>
+ <privatestaticattributes title=""/>
+ <friends title=""/>
+ <related title="" subtitle=""/>
+ <membergroups visible="yes"/>
+ </memberdecl>
+ <detaileddescription title=""/>
+ <memberdef>
+ <inlineclasses title=""/>
+ <typedefs title=""/>
+ <enums title=""/>
+ <services title=""/>
+ <interfaces title=""/>
+ <constructors title=""/>
+ <functions title=""/>
+ <related title=""/>
+ <variables title=""/>
+ <properties title=""/>
+ <events title=""/>
+ </memberdef>
+ <allmemberslink visible="yes"/>
+ <usedfiles visible="$SHOW_USED_FILES"/>
+ <authorsection visible="yes"/>
+ </class>
+
+ <!-- Layout definition for a namespace page -->
+ <namespace>
+ <briefdescription visible="yes"/>
+ <memberdecl>
+ <nestednamespaces visible="yes" title=""/>
+ <constantgroups visible="yes" title=""/>
+ <classes visible="yes" title=""/>
+ <typedefs title=""/>
+ <enums title=""/>
+ <functions title=""/>
+ <variables title=""/>
+ <membergroups visible="yes"/>
+ </memberdecl>
+ <detaileddescription title=""/>
+ <memberdef>
+ <inlineclasses title=""/>
+ <typedefs title=""/>
+ <enums title=""/>
+ <functions title=""/>
+ <variables title=""/>
+ </memberdef>
+ <authorsection visible="yes"/>
+ </namespace>
+
+ <!-- Layout definition for a file page -->
+ <file>
+ <briefdescription visible="yes"/>
+ <includes visible="$SHOW_INCLUDE_FILES"/>
+ <includegraph visible="$INCLUDE_GRAPH"/>
+ <includedbygraph visible="$INCLUDED_BY_GRAPH"/>
+ <sourcelink visible="yes"/>
+ <memberdecl>
+ <classes visible="yes" title=""/>
+ <namespaces visible="yes" title=""/>
+ <constantgroups visible="yes" title=""/>
+ <defines title=""/>
+ <typedefs title=""/>
+ <enums title=""/>
+ <functions title=""/>
+ <variables title=""/>
+ <membergroups visible="yes"/>
+ </memberdecl>
+ <detaileddescription title=""/>
+ <memberdef>
+ <inlineclasses title=""/>
+ <defines title=""/>
+ <typedefs title=""/>
+ <enums title=""/>
+ <functions title=""/>
+ <variables title=""/>
+ </memberdef>
+ <authorsection/>
+ </file>
+
+ <!-- Layout definition for a group page -->
+ <group>
+ <briefdescription visible="yes"/>
+ <groupgraph visible="$GROUP_GRAPHS"/>
+ <memberdecl>
+ <nestedgroups visible="yes" title=""/>
+ <dirs visible="yes" title=""/>
+ <files visible="yes" title=""/>
+ <namespaces visible="yes" title=""/>
+ <classes visible="yes" title=""/>
+ <defines title=""/>
+ <typedefs title=""/>
+ <enums title=""/>
+ <enumvalues title=""/>
+ <functions title=""/>
+ <variables title=""/>
+ <signals title=""/>
+ <publicslots title=""/>
+ <protectedslots title=""/>
+ <privateslots title=""/>
+ <events title=""/>
+ <properties title=""/>
+ <friends title=""/>
+ <membergroups visible="yes"/>
+ </memberdecl>
+ <detaileddescription title=""/>
+ <memberdef>
+ <pagedocs/>
+ <inlineclasses title=""/>
+ <defines title=""/>
+ <typedefs title=""/>
+ <enums title=""/>
+ <enumvalues title=""/>
+ <functions title=""/>
+ <variables title=""/>
+ <signals title=""/>
+ <publicslots title=""/>
+ <protectedslots title=""/>
+ <privateslots title=""/>
+ <events title=""/>
+ <properties title=""/>
+ <friends title=""/>
+ </memberdef>
+ <authorsection visible="yes"/>
+ </group>
+
+ <!-- Layout definition for a directory page -->
+ <directory>
+ <briefdescription visible="yes"/>
+ <directorygraph visible="yes"/>
+ <memberdecl>
+ <dirs visible="yes"/>
+ <files visible="yes"/>
+ </memberdecl>
+ <detaileddescription title=""/>
+ </directory>
+</doxygenlayout>
diff --git a/ct_rbd/doc/ct_rbd.doxyfile b/ct_rbd/doc/ct_rbd.doxyfile
new file mode 100644
index 0000000..a46b671
--- /dev/null
+++ b/ct_rbd/doc/ct_rbd.doxyfile
@@ -0,0 +1,2316 @@
+# Doxyfile 1.8.7
+
+# This file describes the settings to be used by the documentation system
+# doxygen (www.doxygen.org) for a project.
+#
+# All text after a double hash (##) is considered a comment and is placed in
+# front of the TAG it is preceding.
+#
+# All text after a single hash (#) is considered a comment and will be ignored.
+# The format is:
+# TAG = value [value, ...]
+# For lists, items can also be appended using:
+# TAG += value [value, ...]
+# Values that contain spaces should be placed between quotes (\" \").
+
+#---------------------------------------------------------------------------
+# Project related configuration options
+#---------------------------------------------------------------------------
+
+# This tag specifies the encoding used for all characters in the config file
+# that follow. The default is UTF-8 which is also the encoding used for all text
+# before the first occurrence of this tag. Doxygen uses libiconv (or the iconv
+# built into libc) for the transcoding. See http://www.gnu.org/software/libiconv
+# for the list of possible encodings.
+# The default value is: UTF-8.
+
+DOXYFILE_ENCODING = UTF-8
+
+# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by
+# double-quotes, unless you are using Doxywizard) that should identify the
+# project for which the documentation is generated. This name is used in the
+# title of most generated pages and in a few other places.
+# The default value is: My Project.
+
+PROJECT_NAME = ct_rbd
+
+# The PROJECT_NUMBER tag can be used to enter a project or revision number. This
+# could be handy for archiving the generated documentation or if some version
+# control system is used.
+
+PROJECT_NUMBER = v2.0
+
+# Using the PROJECT_BRIEF tag one can provide an optional one line description
+# for a project that appears at the top of each page and should give viewer a
+# quick idea about the purpose of the project. Keep the description short.
+
+PROJECT_BRIEF = "Control Toolbox - Rigid Body Dynamics Module"
+
+# With the PROJECT_LOGO tag one can specify an logo or icon that is included in
+# the documentation. The maximum height of the logo should not exceed 55 pixels
+# and the maximum width should not exceed 200 pixels. Doxygen will copy the logo
+# to the output directory.
+
+PROJECT_LOGO =
+
+# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path
+# into which the generated documentation will be written. If a relative path is
+# entered, it will be relative to the location where doxygen was started. If
+# left blank the current directory will be used.
+
+OUTPUT_DIRECTORY = .
+
+# If the CREATE_SUBDIRS tag is set to YES, then doxygen will create 4096 sub-
+# directories (in 2 levels) under the output directory of each output format and
+# will distribute the generated files over these directories. Enabling this
+# option can be useful when feeding doxygen a huge amount of source files, where
+# putting all generated files in the same directory would otherwise causes
+# performance problems for the file system.
+# The default value is: NO.
+
+CREATE_SUBDIRS = NO
+
+# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII
+# characters to appear in the names of generated files. If set to NO, non-ASCII
+# characters will be escaped, for example _xE3_x81_x84 will be used for Unicode
+# U+3044.
+# The default value is: NO.
+
+ALLOW_UNICODE_NAMES = NO
+
+# The OUTPUT_LANGUAGE tag is used to specify the language in which all
+# documentation generated by doxygen is written. Doxygen will use this
+# information to generate all constant output in the proper language.
+# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese,
+# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States),
+# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian,
+# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages),
+# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian,
+# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian,
+# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish,
+# Ukrainian and Vietnamese.
+# The default value is: English.
+
+OUTPUT_LANGUAGE = English
+
+# If the BRIEF_MEMBER_DESC tag is set to YES doxygen will include brief member
+# descriptions after the members that are listed in the file and class
+# documentation (similar to Javadoc). Set to NO to disable this.
+# The default value is: YES.
+
+BRIEF_MEMBER_DESC = YES
+
+# If the REPEAT_BRIEF tag is set to YES doxygen will prepend the brief
+# description of a member or function before the detailed description
+#
+# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the
+# brief descriptions will be completely suppressed.
+# The default value is: YES.
+
+REPEAT_BRIEF = YES
+
+# This tag implements a quasi-intelligent brief description abbreviator that is
+# used to form the text in various listings. Each string in this list, if found
+# as the leading text of the brief description, will be stripped from the text
+# and the result, after processing the whole list, is used as the annotated
+# text. Otherwise, the brief description is used as-is. If left blank, the
+# following values are used ($name is automatically replaced with the name of
+# the entity):The $name class, The $name widget, The $name file, is, provides,
+# specifies, contains, represents, a, an and the.
+
+ABBREVIATE_BRIEF =
+
+# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then
+# doxygen will generate a detailed section even if there is only a brief
+# description.
+# The default value is: NO.
+
+ALWAYS_DETAILED_SEC = NO
+
+# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all
+# inherited members of a class in the documentation of that class as if those
+# members were ordinary class members. Constructors, destructors and assignment
+# operators of the base classes will not be shown.
+# The default value is: NO.
+
+INLINE_INHERITED_MEMB = NO
+
+# If the FULL_PATH_NAMES tag is set to YES doxygen will prepend the full path
+# before files name in the file list and in the header files. If set to NO the
+# shortest path that makes the file name unique will be used
+# The default value is: YES.
+
+FULL_PATH_NAMES = YES
+
+# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path.
+# Stripping is only done if one of the specified strings matches the left-hand
+# part of the path. The tag can be used to show relative paths in the file list.
+# If left blank the directory from which doxygen is run is used as the path to
+# strip.
+#
+# Note that you can specify absolute paths here, but also relative paths, which
+# will be relative from the directory where doxygen is started.
+# This tag requires that the tag FULL_PATH_NAMES is set to YES.
+
+STRIP_FROM_PATH =
+
+# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the
+# path mentioned in the documentation of a class, which tells the reader which
+# header file to include in order to use a class. If left blank only the name of
+# the header file containing the class definition is used. Otherwise one should
+# specify the list of include paths that are normally passed to the compiler
+# using the -I flag.
+
+STRIP_FROM_INC_PATH =
+
+# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but
+# less readable) file names. This can be useful is your file systems doesn't
+# support long names like on DOS, Mac, or CD-ROM.
+# The default value is: NO.
+
+SHORT_NAMES = NO
+
+# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the
+# first line (until the first dot) of a Javadoc-style comment as the brief
+# description. If set to NO, the Javadoc-style will behave just like regular Qt-
+# style comments (thus requiring an explicit @brief command for a brief
+# description.)
+# The default value is: NO.
+
+JAVADOC_AUTOBRIEF = NO
+
+# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first
+# line (until the first dot) of a Qt-style comment as the brief description. If
+# set to NO, the Qt-style will behave just like regular Qt-style comments (thus
+# requiring an explicit \brief command for a brief description.)
+# The default value is: NO.
+
+QT_AUTOBRIEF = NO
+
+# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a
+# multi-line C++ special comment block (i.e. a block of //! or /// comments) as
+# a brief description. This used to be the default behavior. The new default is
+# to treat a multi-line C++ comment block as a detailed description. Set this
+# tag to YES if you prefer the old behavior instead.
+#
+# Note that setting this tag to YES also means that rational rose comments are
+# not recognized any more.
+# The default value is: NO.
+
+MULTILINE_CPP_IS_BRIEF = NO
+
+# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the
+# documentation from any documented member that it re-implements.
+# The default value is: YES.
+
+INHERIT_DOCS = YES
+
+# If the SEPARATE_MEMBER_PAGES tag is set to YES, then doxygen will produce a
+# new page for each member. If set to NO, the documentation of a member will be
+# part of the file/class/namespace that contains it.
+# The default value is: NO.
+
+SEPARATE_MEMBER_PAGES = NO
+
+# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen
+# uses this value to replace tabs by spaces in code fragments.
+# Minimum value: 1, maximum value: 16, default value: 4.
+
+TAB_SIZE = 4
+
+# This tag can be used to specify a number of aliases that act as commands in
+# the documentation. An alias has the form:
+# name=value
+# For example adding
+# "sideeffect=@par Side Effects:\n"
+# will allow you to put the command \sideeffect (or @sideeffect) in the
+# documentation, which will result in a user-defined paragraph with heading
+# "Side Effects:". You can put \n's in the value part of an alias to insert
+# newlines.
+
+ALIASES =
+
+# This tag can be used to specify a number of word-keyword mappings (TCL only).
+# A mapping has the form "name=value". For example adding "class=itcl::class"
+# will allow you to use the command class in the itcl::class meaning.
+
+TCL_SUBST =
+
+# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources
+# only. Doxygen will then generate output that is more tailored for C. For
+# instance, some of the names that are used will be different. The list of all
+# members will be omitted, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_FOR_C = NO
+
+# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or
+# Python sources only. Doxygen will then generate output that is more tailored
+# for that language. For instance, namespaces will be presented as packages,
+# qualified scopes will look different, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_JAVA = NO
+
+# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran
+# sources. Doxygen will then generate output that is tailored for Fortran.
+# The default value is: NO.
+
+OPTIMIZE_FOR_FORTRAN = NO
+
+# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL
+# sources. Doxygen will then generate output that is tailored for VHDL.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_VHDL = NO
+
+# Doxygen selects the parser to use depending on the extension of the files it
+# parses. With this tag you can assign which parser to use for a given
+# extension. Doxygen has a built-in mapping, but you can override or extend it
+# using this tag. The format is ext=language, where ext is a file extension, and
+# language is one of the parsers supported by doxygen: IDL, Java, Javascript,
+# C#, C, C++, D, PHP, Objective-C, Python, Fortran (fixed format Fortran:
+# FortranFixed, free formatted Fortran: FortranFree, unknown formatted Fortran:
+# Fortran. In the later case the parser tries to guess whether the code is fixed
+# or free formatted code, this is the default for Fortran type files), VHDL. For
+# instance to make doxygen treat .inc files as Fortran files (default is PHP),
+# and .f files as C (default is Fortran), use: inc=Fortran f=C.
+#
+# Note For files without extension you can use no_extension as a placeholder.
+#
+# Note that for custom extensions you also need to set FILE_PATTERNS otherwise
+# the files are not read by doxygen.
+
+EXTENSION_MAPPING =
+
+# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments
+# according to the Markdown format, which allows for more readable
+# documentation. See http://daringfireball.net/projects/markdown/ for details.
+# The output of markdown processing is further processed by doxygen, so you can
+# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in
+# case of backward compatibilities issues.
+# The default value is: YES.
+
+MARKDOWN_SUPPORT = YES
+
+# When enabled doxygen tries to link words that correspond to documented
+# classes, or namespaces to their corresponding documentation. Such a link can
+# be prevented in individual cases by by putting a % sign in front of the word
+# or globally by setting AUTOLINK_SUPPORT to NO.
+# The default value is: YES.
+
+AUTOLINK_SUPPORT = YES
+
+# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want
+# to include (a tag file for) the STL sources as input, then you should set this
+# tag to YES in order to let doxygen match functions declarations and
+# definitions whose arguments contain STL classes (e.g. func(std::string);
+# versus func(std::string) {}). This also make the inheritance and collaboration
+# diagrams that involve STL classes more complete and accurate.
+# The default value is: NO.
+
+BUILTIN_STL_SUPPORT = NO
+
+# If you use Microsoft's C++/CLI language, you should set this option to YES to
+# enable parsing support.
+# The default value is: NO.
+
+CPP_CLI_SUPPORT = NO
+
+# Set the SIP_SUPPORT tag to YES if your project consists of sip (see:
+# http://www.riverbankcomputing.co.uk/software/sip/intro) sources only. Doxygen
+# will parse them like normal C++ but will assume all classes use public instead
+# of private inheritance when no explicit protection keyword is present.
+# The default value is: NO.
+
+SIP_SUPPORT = NO
+
+# For Microsoft's IDL there are propget and propput attributes to indicate
+# getter and setter methods for a property. Setting this option to YES will make
+# doxygen to replace the get and set methods by a property in the documentation.
+# This will only work if the methods are indeed getting or setting a simple
+# type. If this is not the case, or you want to show the methods anyway, you
+# should set this option to NO.
+# The default value is: YES.
+
+IDL_PROPERTY_SUPPORT = YES
+
+# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC
+# tag is set to YES, then doxygen will reuse the documentation of the first
+# member in the group (if any) for the other members of the group. By default
+# all members of a group must be documented explicitly.
+# The default value is: NO.
+
+DISTRIBUTE_GROUP_DOC = YES
+
+# Set the SUBGROUPING tag to YES to allow class member groups of the same type
+# (for instance a group of public functions) to be put as a subgroup of that
+# type (e.g. under the Public Functions section). Set it to NO to prevent
+# subgrouping. Alternatively, this can be done per class using the
+# \nosubgrouping command.
+# The default value is: YES.
+
+SUBGROUPING = YES
+
+# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions
+# are shown inside the group in which they are included (e.g. using \ingroup)
+# instead of on a separate page (for HTML and Man pages) or section (for LaTeX
+# and RTF).
+#
+# Note that this feature does not work in combination with
+# SEPARATE_MEMBER_PAGES.
+# The default value is: NO.
+
+INLINE_GROUPED_CLASSES = NO
+
+# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions
+# with only public data fields or simple typedef fields will be shown inline in
+# the documentation of the scope in which they are defined (i.e. file,
+# namespace, or group documentation), provided this scope is documented. If set
+# to NO, structs, classes, and unions are shown on a separate page (for HTML and
+# Man pages) or section (for LaTeX and RTF).
+# The default value is: NO.
+
+INLINE_SIMPLE_STRUCTS = NO
+
+# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or
+# enum is documented as struct, union, or enum with the name of the typedef. So
+# typedef struct TypeS {} TypeT, will appear in the documentation as a struct
+# with name TypeT. When disabled the typedef will appear as a member of a file,
+# namespace, or class. And the struct will be named TypeS. This can typically be
+# useful for C code in case the coding convention dictates that all compound
+# types are typedef'ed and only the typedef is referenced, never the tag name.
+# The default value is: NO.
+
+TYPEDEF_HIDES_STRUCT = NO
+
+# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This
+# cache is used to resolve symbols given their name and scope. Since this can be
+# an expensive process and often the same symbol appears multiple times in the
+# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small
+# doxygen will become slower. If the cache is too large, memory is wasted. The
+# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range
+# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536
+# symbols. At the end of a run doxygen will report the cache usage and suggest
+# the optimal cache size from a speed point of view.
+# Minimum value: 0, maximum value: 9, default value: 0.
+
+LOOKUP_CACHE_SIZE = 0
+
+#---------------------------------------------------------------------------
+# Build related configuration options
+#---------------------------------------------------------------------------
+
+# If the EXTRACT_ALL tag is set to YES doxygen will assume all entities in
+# documentation are documented, even if no documentation was available. Private
+# class members and static file members will be hidden unless the
+# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES.
+# Note: This will also disable the warnings about undocumented members that are
+# normally produced when WARNINGS is set to YES.
+# The default value is: NO.
+
+EXTRACT_ALL = YES
+
+# If the EXTRACT_PRIVATE tag is set to YES all private members of a class will
+# be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PRIVATE = NO
+
+# If the EXTRACT_PACKAGE tag is set to YES all members with package or internal
+# scope will be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PACKAGE = NO
+
+# If the EXTRACT_STATIC tag is set to YES all static members of a file will be
+# included in the documentation.
+# The default value is: NO.
+
+EXTRACT_STATIC = NO
+
+# If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) defined
+# locally in source files will be included in the documentation. If set to NO
+# only classes defined in header files are included. Does not have any effect
+# for Java sources.
+# The default value is: YES.
+
+EXTRACT_LOCAL_CLASSES = YES
+
+# This flag is only useful for Objective-C code. When set to YES local methods,
+# which are defined in the implementation section but not in the interface are
+# included in the documentation. If set to NO only methods in the interface are
+# included.
+# The default value is: NO.
+
+EXTRACT_LOCAL_METHODS = NO
+
+# If this flag is set to YES, the members of anonymous namespaces will be
+# extracted and appear in the documentation as a namespace called
+# 'anonymous_namespace{file}', where file will be replaced with the base name of
+# the file that contains the anonymous namespace. By default anonymous namespace
+# are hidden.
+# The default value is: NO.
+
+EXTRACT_ANON_NSPACES = NO
+
+# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all
+# undocumented members inside documented classes or files. If set to NO these
+# members will be included in the various overviews, but no documentation
+# section is generated. This option has no effect if EXTRACT_ALL is enabled.
+# The default value is: NO.
+
+HIDE_UNDOC_MEMBERS = YES
+
+# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all
+# undocumented classes that are normally visible in the class hierarchy. If set
+# to NO these classes will be included in the various overviews. This option has
+# no effect if EXTRACT_ALL is enabled.
+# The default value is: NO.
+
+HIDE_UNDOC_CLASSES = YES
+
+# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend
+# (class|struct|union) declarations. If set to NO these declarations will be
+# included in the documentation.
+# The default value is: NO.
+
+HIDE_FRIEND_COMPOUNDS = NO
+
+# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any
+# documentation blocks found inside the body of a function. If set to NO these
+# blocks will be appended to the function's detailed documentation block.
+# The default value is: NO.
+
+HIDE_IN_BODY_DOCS = NO
+
+# The INTERNAL_DOCS tag determines if documentation that is typed after a
+# \internal command is included. If the tag is set to NO then the documentation
+# will be excluded. Set it to YES to include the internal documentation.
+# The default value is: NO.
+
+INTERNAL_DOCS = NO
+
+# If the CASE_SENSE_NAMES tag is set to NO then doxygen will only generate file
+# names in lower-case letters. If set to YES upper-case letters are also
+# allowed. This is useful if you have classes or files whose names only differ
+# in case and if your file system supports case sensitive file names. Windows
+# and Mac users are advised to set this option to NO.
+# The default value is: system dependent.
+
+CASE_SENSE_NAMES = YES
+
+# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with
+# their full class and namespace scopes in the documentation. If set to YES the
+# scope will be hidden.
+# The default value is: NO.
+
+HIDE_SCOPE_NAMES = NO
+
+# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of
+# the files that are included by a file in the documentation of that file.
+# The default value is: YES.
+
+SHOW_INCLUDE_FILES = YES
+
+# If the SHOW_GROUPED_MEMB_INC tag is set to YES then Doxygen will add for each
+# grouped member an include statement to the documentation, telling the reader
+# which file to include in order to use the member.
+# The default value is: NO.
+
+SHOW_GROUPED_MEMB_INC = NO
+
+# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include
+# files with double quotes in the documentation rather than with sharp brackets.
+# The default value is: NO.
+
+FORCE_LOCAL_INCLUDES = NO
+
+# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the
+# documentation for inline members.
+# The default value is: YES.
+
+INLINE_INFO = YES
+
+# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the
+# (detailed) documentation of file and class members alphabetically by member
+# name. If set to NO the members will appear in declaration order.
+# The default value is: YES.
+
+SORT_MEMBER_DOCS = NO
+
+# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief
+# descriptions of file, namespace and class members alphabetically by member
+# name. If set to NO the members will appear in declaration order. Note that
+# this will also influence the order of the classes in the class list.
+# The default value is: NO.
+
+SORT_BRIEF_DOCS = NO
+
+# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the
+# (brief and detailed) documentation of class members so that constructors and
+# destructors are listed first. If set to NO the constructors will appear in the
+# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS.
+# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief
+# member documentation.
+# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting
+# detailed member documentation.
+# The default value is: NO.
+
+SORT_MEMBERS_CTORS_1ST = NO
+
+# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy
+# of group names into alphabetical order. If set to NO the group names will
+# appear in their defined order.
+# The default value is: NO.
+
+SORT_GROUP_NAMES = NO
+
+# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by
+# fully-qualified names, including namespaces. If set to NO, the class list will
+# be sorted only by class name, not including the namespace part.
+# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES.
+# Note: This option applies only to the class list, not to the alphabetical
+# list.
+# The default value is: NO.
+
+SORT_BY_SCOPE_NAME = NO
+
+# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper
+# type resolution of all parameters of a function it will reject a match between
+# the prototype and the implementation of a member function even if there is
+# only one candidate or it is obvious which candidate to choose by doing a
+# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still
+# accept a match between prototype and implementation in such cases.
+# The default value is: NO.
+
+STRICT_PROTO_MATCHING = NO
+
+# The GENERATE_TODOLIST tag can be used to enable ( YES) or disable ( NO) the
+# todo list. This list is created by putting \todo commands in the
+# documentation.
+# The default value is: YES.
+
+GENERATE_TODOLIST = YES
+
+# The GENERATE_TESTLIST tag can be used to enable ( YES) or disable ( NO) the
+# test list. This list is created by putting \test commands in the
+# documentation.
+# The default value is: YES.
+
+GENERATE_TESTLIST = YES
+
+# The GENERATE_BUGLIST tag can be used to enable ( YES) or disable ( NO) the bug
+# list. This list is created by putting \bug commands in the documentation.
+# The default value is: YES.
+
+GENERATE_BUGLIST = YES
+
+# The GENERATE_DEPRECATEDLIST tag can be used to enable ( YES) or disable ( NO)
+# the deprecated list. This list is created by putting \deprecated commands in
+# the documentation.
+# The default value is: YES.
+
+GENERATE_DEPRECATEDLIST= YES
+
+# The ENABLED_SECTIONS tag can be used to enable conditional documentation
+# sections, marked by \if <section_label> ... \endif and \cond <section_label>
+# ... \endcond blocks.
+
+ENABLED_SECTIONS =
+
+# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the
+# initial value of a variable or macro / define can have for it to appear in the
+# documentation. If the initializer consists of more lines than specified here
+# it will be hidden. Use a value of 0 to hide initializers completely. The
+# appearance of the value of individual variables and macros / defines can be
+# controlled using \showinitializer or \hideinitializer command in the
+# documentation regardless of this setting.
+# Minimum value: 0, maximum value: 10000, default value: 30.
+
+MAX_INITIALIZER_LINES = 30
+
+# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at
+# the bottom of the documentation of classes and structs. If set to YES the list
+# will mention the files that were used to generate the documentation.
+# The default value is: YES.
+
+SHOW_USED_FILES = YES
+
+# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This
+# will remove the Files entry from the Quick Index and from the Folder Tree View
+# (if specified).
+# The default value is: YES.
+
+SHOW_FILES = YES
+
+# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces
+# page. This will remove the Namespaces entry from the Quick Index and from the
+# Folder Tree View (if specified).
+# The default value is: YES.
+
+SHOW_NAMESPACES = YES
+
+# The FILE_VERSION_FILTER tag can be used to specify a program or script that
+# doxygen should invoke to get the current version for each file (typically from
+# the version control system). Doxygen will invoke the program by executing (via
+# popen()) the command command input-file, where command is the value of the
+# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided
+# by doxygen. Whatever the program writes to standard output is used as the file
+# version. For an example see the documentation.
+
+FILE_VERSION_FILTER =
+
+# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed
+# by doxygen. The layout file controls the global structure of the generated
+# output files in an output format independent way. To create the layout file
+# that represents doxygen's defaults, run doxygen with the -l option. You can
+# optionally specify a file name after the option, if omitted DoxygenLayout.xml
+# will be used as the name of the layout file.
+#
+# Note that if you run doxygen from a directory containing a file called
+# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE
+# tag is left empty.
+
+LAYOUT_FILE = DoxygenLayout.xml
+
+# The CITE_BIB_FILES tag can be used to specify one or more bib files containing
+# the reference definitions. This must be a list of .bib files. The .bib
+# extension is automatically appended if omitted. This requires the bibtex tool
+# to be installed. See also http://en.wikipedia.org/wiki/BibTeX for more info.
+# For LaTeX the style of the bibliography can be controlled using
+# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the
+# search path. Do not use file names with spaces, bibtex cannot handle them. See
+# also \cite for info how to create references.
+
+CITE_BIB_FILES =
+
+#---------------------------------------------------------------------------
+# Configuration options related to warning and progress messages
+#---------------------------------------------------------------------------
+
+# The QUIET tag can be used to turn on/off the messages that are generated to
+# standard output by doxygen. If QUIET is set to YES this implies that the
+# messages are off.
+# The default value is: NO.
+
+QUIET = NO
+
+# The WARNINGS tag can be used to turn on/off the warning messages that are
+# generated to standard error ( stderr) by doxygen. If WARNINGS is set to YES
+# this implies that the warnings are on.
+#
+# Tip: Turn warnings on while writing the documentation.
+# The default value is: YES.
+
+WARNINGS = YES
+
+# If the WARN_IF_UNDOCUMENTED tag is set to YES, then doxygen will generate
+# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag
+# will automatically be disabled.
+# The default value is: YES.
+
+WARN_IF_UNDOCUMENTED = YES
+
+# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for
+# potential errors in the documentation, such as not documenting some parameters
+# in a documented function, or documenting parameters that don't exist or using
+# markup commands wrongly.
+# The default value is: YES.
+
+WARN_IF_DOC_ERROR = YES
+
+# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that
+# are documented, but have no documentation for their parameters or return
+# value. If set to NO doxygen will only warn about wrong or incomplete parameter
+# documentation, but not about the absence of documentation.
+# The default value is: NO.
+
+WARN_NO_PARAMDOC = NO
+
+# The WARN_FORMAT tag determines the format of the warning messages that doxygen
+# can produce. The string should contain the $file, $line, and $text tags, which
+# will be replaced by the file and line number from which the warning originated
+# and the warning text. Optionally the format may contain $version, which will
+# be replaced by the version of the file (if it could be obtained via
+# FILE_VERSION_FILTER)
+# The default value is: $file:$line: $text.
+
+WARN_FORMAT = "$file:$line: $text"
+
+# The WARN_LOGFILE tag can be used to specify a file to which warning and error
+# messages should be written. If left blank the output is written to standard
+# error (stderr).
+
+WARN_LOGFILE =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the input files
+#---------------------------------------------------------------------------
+
+# The INPUT tag is used to specify the files and/or directories that contain
+# documented source files. You may enter file names like myfile.cpp or
+# directories like /usr/src/myproject. Separate the files or directories with
+# spaces.
+# Note: If this tag is empty the current directory is searched.
+
+INPUT = ../ ./
+
+# This tag can be used to specify the character encoding of the source files
+# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses
+# libiconv (or the iconv built into libc) for the transcoding. See the libiconv
+# documentation (see: http://www.gnu.org/software/libiconv) for the list of
+# possible encodings.
+# The default value is: UTF-8.
+
+INPUT_ENCODING = UTF-8
+
+# If the value of the INPUT tag contains directories, you can use the
+# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and
+# *.h) to filter out the source-files in the directories. If left blank the
+# following patterns are tested:*.c, *.cc, *.cxx, *.cpp, *.c++, *.java, *.ii,
+# *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h, *.hh, *.hxx, *.hpp,
+# *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc, *.m, *.markdown,
+# *.md, *.mm, *.dox, *.py, *.f90, *.f, *.for, *.tcl, *.vhd, *.vhdl, *.ucf,
+# *.qsf, *.as and *.js.
+
+FILE_PATTERNS =
+
+# The RECURSIVE tag can be used to specify whether or not subdirectories should
+# be searched for input files as well.
+# The default value is: NO.
+
+RECURSIVE = YES
+
+# The EXCLUDE tag can be used to specify files and/or directories that should be
+# excluded from the INPUT source files. This way you can easily exclude a
+# subdirectory from a directory tree whose root is specified with the INPUT tag.
+#
+# Note that relative paths are relative to the directory from which doxygen is
+# run.
+
+EXCLUDE =
+
+# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or
+# directories that are symbolic links (a Unix file system feature) are excluded
+# from the input.
+# The default value is: NO.
+
+EXCLUDE_SYMLINKS = NO
+
+# If the value of the INPUT tag contains directories, you can use the
+# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude
+# certain files from those directories.
+#
+# Note that the wildcards are matched against the file with absolute path, so to
+# exclude all test directories for example use the pattern */test/*
+
+EXCLUDE_PATTERNS = */external/cppad/* \
+ */src/codegen/* \
+ */generated/jacobians* \
+ */generated/transforms*
+
+# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names
+# (namespaces, classes, functions, etc.) that should be excluded from the
+# output. The symbol name can be a fully qualified name, a word, or if the
+# wildcard * is used, a substring. Examples: ANamespace, AClass,
+# AClass::ANamespace, ANamespace::*Test
+#
+# Note that the wildcards are matched against the file with absolute path, so to
+# exclude all test directories use the pattern */test/*
+
+EXCLUDE_SYMBOLS =
+
+# The EXAMPLE_PATH tag can be used to specify one or more files or directories
+# that contain example code fragments that are included (see the \include
+# command).
+
+EXAMPLE_PATH = ../test
+
+# If the value of the EXAMPLE_PATH tag contains directories, you can use the
+# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and
+# *.h) to filter out the source-files in the directories. If left blank all
+# files are included.
+
+EXAMPLE_PATTERNS = *.cpp
+
+# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be
+# searched for input files to be used with the \include or \dontinclude commands
+# irrespective of the value of the RECURSIVE tag.
+# The default value is: NO.
+
+EXAMPLE_RECURSIVE = YES
+
+# The IMAGE_PATH tag can be used to specify one or more files or directories
+# that contain images that are to be included in the documentation (see the
+# \image command).
+
+IMAGE_PATH =
+
+# The INPUT_FILTER tag can be used to specify a program that doxygen should
+# invoke to filter for each input file. Doxygen will invoke the filter program
+# by executing (via popen()) the command:
+#
+# <filter> <input-file>
+#
+# where <filter> is the value of the INPUT_FILTER tag, and <input-file> is the
+# name of an input file. Doxygen will then use the output that the filter
+# program writes to standard output. If FILTER_PATTERNS is specified, this tag
+# will be ignored.
+#
+# Note that the filter must not add or remove lines; it is applied before the
+# code is scanned, but not when the output code is generated. If lines are added
+# or removed, the anchors will not be placed correctly.
+
+INPUT_FILTER =
+
+# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern
+# basis. Doxygen will compare the file name with each pattern and apply the
+# filter if there is a match. The filters are a list of the form: pattern=filter
+# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how
+# filters are used. If the FILTER_PATTERNS tag is empty or if none of the
+# patterns match the file name, INPUT_FILTER is applied.
+
+FILTER_PATTERNS =
+
+# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using
+# INPUT_FILTER ) will also be used to filter the input files that are used for
+# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES).
+# The default value is: NO.
+
+FILTER_SOURCE_FILES = NO
+
+# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file
+# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and
+# it is also possible to disable source filtering for a specific pattern using
+# *.ext= (so without naming a filter).
+# This tag requires that the tag FILTER_SOURCE_FILES is set to YES.
+
+FILTER_SOURCE_PATTERNS =
+
+# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that
+# is part of the input, its contents will be placed on the main page
+# (index.html). This can be useful if you have a project on for instance GitHub
+# and want to reuse the introduction page also for the doxygen output.
+
+USE_MDFILE_AS_MAINPAGE =
+
+#---------------------------------------------------------------------------
+# Configuration options related to source browsing
+#---------------------------------------------------------------------------
+
+# If the SOURCE_BROWSER tag is set to YES then a list of source files will be
+# generated. Documented entities will be cross-referenced with these sources.
+#
+# Note: To get rid of all source code in the generated output, make sure that
+# also VERBATIM_HEADERS is set to NO.
+# The default value is: NO.
+
+SOURCE_BROWSER = NO
+
+# Setting the INLINE_SOURCES tag to YES will include the body of functions,
+# classes and enums directly into the documentation.
+# The default value is: NO.
+
+INLINE_SOURCES = NO
+
+# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any
+# special comment blocks from generated source code fragments. Normal C, C++ and
+# Fortran comments will always remain visible.
+# The default value is: YES.
+
+STRIP_CODE_COMMENTS = YES
+
+# If the REFERENCED_BY_RELATION tag is set to YES then for each documented
+# function all documented functions referencing it will be listed.
+# The default value is: NO.
+
+REFERENCED_BY_RELATION = YES
+
+# If the REFERENCES_RELATION tag is set to YES then for each documented function
+# all documented entities called/used by that function will be listed.
+# The default value is: NO.
+
+REFERENCES_RELATION = YES
+
+# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set
+# to YES, then the hyperlinks from functions in REFERENCES_RELATION and
+# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will
+# link to the documentation.
+# The default value is: YES.
+
+REFERENCES_LINK_SOURCE = YES
+
+# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the
+# source code will show a tooltip with additional information such as prototype,
+# brief description and links to the definition and documentation. Since this
+# will make the HTML file larger and loading of large files a bit slower, you
+# can opt to disable this feature.
+# The default value is: YES.
+# This tag requires that the tag SOURCE_BROWSER is set to YES.
+
+SOURCE_TOOLTIPS = YES
+
+# If the USE_HTAGS tag is set to YES then the references to source code will
+# point to the HTML generated by the htags(1) tool instead of doxygen built-in
+# source browser. The htags tool is part of GNU's global source tagging system
+# (see http://www.gnu.org/software/global/global.html). You will need version
+# 4.8.6 or higher.
+#
+# To use it do the following:
+# - Install the latest version of global
+# - Enable SOURCE_BROWSER and USE_HTAGS in the config file
+# - Make sure the INPUT points to the root of the source tree
+# - Run doxygen as normal
+#
+# Doxygen will invoke htags (and that will in turn invoke gtags), so these
+# tools must be available from the command line (i.e. in the search path).
+#
+# The result: instead of the source browser generated by doxygen, the links to
+# source code will now point to the output of htags.
+# The default value is: NO.
+# This tag requires that the tag SOURCE_BROWSER is set to YES.
+
+USE_HTAGS = NO
+
+# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a
+# verbatim copy of the header file for each class for which an include is
+# specified. Set to NO to disable this.
+# See also: Section \class.
+# The default value is: YES.
+
+VERBATIM_HEADERS = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to the alphabetical class index
+#---------------------------------------------------------------------------
+
+# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all
+# compounds will be generated. Enable this if the project contains a lot of
+# classes, structs, unions or interfaces.
+# The default value is: YES.
+
+ALPHABETICAL_INDEX = YES
+
+# The COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns in
+# which the alphabetical index list will be split.
+# Minimum value: 1, maximum value: 20, default value: 5.
+# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
+
+COLS_IN_ALPHA_INDEX = 5
+
+# In case all classes in a project start with a common prefix, all classes will
+# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag
+# can be used to specify a prefix (or a list of prefixes) that should be ignored
+# while generating the index headers.
+# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
+
+IGNORE_PREFIX =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the HTML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_HTML tag is set to YES doxygen will generate HTML output
+# The default value is: YES.
+
+GENERATE_HTML = YES
+
+# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: html.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_OUTPUT = html
+
+# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each
+# generated HTML page (for example: .htm, .php, .asp).
+# The default value is: .html.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FILE_EXTENSION = .html
+
+# The HTML_HEADER tag can be used to specify a user-defined HTML header file for
+# each generated HTML page. If the tag is left blank doxygen will generate a
+# standard header.
+#
+# To get valid HTML the header file that includes any scripts and style sheets
+# that doxygen needs, which is dependent on the configuration options used (e.g.
+# the setting GENERATE_TREEVIEW). It is highly recommended to start with a
+# default header using
+# doxygen -w html new_header.html new_footer.html new_stylesheet.css
+# YourConfigFile
+# and then modify the file new_header.html. See also section "Doxygen usage"
+# for information on how to generate the default header that doxygen normally
+# uses.
+# Note: The header is subject to change so you typically have to regenerate the
+# default header when upgrading to a newer version of doxygen. For a description
+# of the possible markers and block names see the documentation.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_HEADER =
+
+# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each
+# generated HTML page. If the tag is left blank doxygen will generate a standard
+# footer. See HTML_HEADER for more information on how to generate a default
+# footer and what special commands can be used inside the footer. See also
+# section "Doxygen usage" for information on how to generate the default footer
+# that doxygen normally uses.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FOOTER =
+
+# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style
+# sheet that is used by each HTML page. It can be used to fine-tune the look of
+# the HTML output. If left blank doxygen will generate a default style sheet.
+# See also section "Doxygen usage" for information on how to generate the style
+# sheet that doxygen normally uses.
+# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as
+# it is more robust and this tag (HTML_STYLESHEET) will in the future become
+# obsolete.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_STYLESHEET =
+
+# The HTML_EXTRA_STYLESHEET tag can be used to specify an additional user-
+# defined cascading style sheet that is included after the standard style sheets
+# created by doxygen. Using this option one can overrule certain style aspects.
+# This is preferred over using HTML_STYLESHEET since it does not replace the
+# standard style sheet and is therefor more robust against future updates.
+# Doxygen will copy the style sheet file to the output directory. For an example
+# see the documentation.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_EXTRA_STYLESHEET =
+
+# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or
+# other source files which should be copied to the HTML output directory. Note
+# that these files will be copied to the base HTML output directory. Use the
+# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these
+# files. In the HTML_STYLESHEET file, use the file name only. Also note that the
+# files will be copied as-is; there are no commands or markers available.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_EXTRA_FILES =
+
+# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen
+# will adjust the colors in the stylesheet and background images according to
+# this color. Hue is specified as an angle on a colorwheel, see
+# http://en.wikipedia.org/wiki/Hue for more information. For instance the value
+# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300
+# purple, and 360 is red again.
+# Minimum value: 0, maximum value: 359, default value: 220.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_HUE = 290
+
+# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors
+# in the HTML output. For a value of 0 the output will use grayscales only. A
+# value of 255 will produce the most vivid colors.
+# Minimum value: 0, maximum value: 255, default value: 100.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_SAT = 100
+
+# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the
+# luminance component of the colors in the HTML output. Values below 100
+# gradually make the output lighter, whereas values above 100 make the output
+# darker. The value divided by 100 is the actual gamma applied, so 80 represents
+# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not
+# change the gamma.
+# Minimum value: 40, maximum value: 240, default value: 80.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_GAMMA = 80
+
+# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML
+# page will contain the date and time when the page was generated. Setting this
+# to NO can help when comparing the output of multiple runs.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_TIMESTAMP = YES
+
+# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML
+# documentation will contain sections that can be hidden and shown after the
+# page has loaded.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_DYNAMIC_SECTIONS = NO
+
+# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries
+# shown in the various tree structured indices initially; the user can expand
+# and collapse entries dynamically later on. Doxygen will expand the tree to
+# such a level that at most the specified number of entries are visible (unless
+# a fully collapsed tree already exceeds this amount). So setting the number of
+# entries 1 will produce a full collapsed tree by default. 0 is a special value
+# representing an infinite number of entries and will result in a full expanded
+# tree by default.
+# Minimum value: 0, maximum value: 9999, default value: 100.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_INDEX_NUM_ENTRIES = 100
+
+# If the GENERATE_DOCSET tag is set to YES, additional index files will be
+# generated that can be used as input for Apple's Xcode 3 integrated development
+# environment (see: http://developer.apple.com/tools/xcode/), introduced with
+# OSX 10.5 (Leopard). To create a documentation set, doxygen will generate a
+# Makefile in the HTML output directory. Running make will produce the docset in
+# that directory and running make install will install the docset in
+# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at
+# startup. See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html
+# for more information.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_DOCSET = NO
+
+# This tag determines the name of the docset feed. A documentation feed provides
+# an umbrella under which multiple documentation sets from a single provider
+# (such as a company or product suite) can be grouped.
+# The default value is: Doxygen generated docs.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_FEEDNAME = "Doxygen generated docs"
+
+# This tag specifies a string that should uniquely identify the documentation
+# set bundle. This should be a reverse domain-name style string, e.g.
+# com.mycompany.MyDocSet. Doxygen will append .docset to the name.
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_BUNDLE_ID = org.doxygen.Project
+
+# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify
+# the documentation publisher. This should be a reverse domain-name style
+# string, e.g. com.mycompany.MyDocSet.documentation.
+# The default value is: org.doxygen.Publisher.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_PUBLISHER_ID = org.doxygen.Publisher
+
+# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher.
+# The default value is: Publisher.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_PUBLISHER_NAME = Publisher
+
+# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three
+# additional HTML index files: index.hhp, index.hhc, and index.hhk. The
+# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop
+# (see: http://www.microsoft.com/en-us/download/details.aspx?id=21138) on
+# Windows.
+#
+# The HTML Help Workshop contains a compiler that can convert all HTML output
+# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML
+# files are now used as the Windows 98 help format, and will replace the old
+# Windows help format (.hlp) on all Windows platforms in the future. Compressed
+# HTML files also contain an index, a table of contents, and you can search for
+# words in the documentation. The HTML workshop also contains a viewer for
+# compressed HTML files.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_HTMLHELP = NO
+
+# The CHM_FILE tag can be used to specify the file name of the resulting .chm
+# file. You can add a path in front of the file if the result should not be
+# written to the html output directory.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+CHM_FILE =
+
+# The HHC_LOCATION tag can be used to specify the location (absolute path
+# including file name) of the HTML help compiler ( hhc.exe). If non-empty
+# doxygen will try to run the HTML help compiler on the generated index.hhp.
+# The file has to be specified with full path.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+HHC_LOCATION =
+
+# The GENERATE_CHI flag controls if a separate .chi index file is generated (
+# YES) or that it should be included in the master .chm file ( NO).
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+GENERATE_CHI = NO
+
+# The CHM_INDEX_ENCODING is used to encode HtmlHelp index ( hhk), content ( hhc)
+# and project file content.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+CHM_INDEX_ENCODING =
+
+# The BINARY_TOC flag controls whether a binary table of contents is generated (
+# YES) or a normal table of contents ( NO) in the .chm file. Furthermore it
+# enables the Previous and Next buttons.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+BINARY_TOC = NO
+
+# The TOC_EXPAND flag can be set to YES to add extra items for group members to
+# the table of contents of the HTML help documentation and to the tree view.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+TOC_EXPAND = NO
+
+# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and
+# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that
+# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help
+# (.qch) of the generated HTML documentation.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_QHP = NO
+
+# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify
+# the file name of the resulting .qch file. The path specified is relative to
+# the HTML output folder.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QCH_FILE =
+
+# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help
+# Project output. For more information please see Qt Help Project / Namespace
+# (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#namespace).
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_NAMESPACE = org.doxygen.Project
+
+# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt
+# Help Project output. For more information please see Qt Help Project / Virtual
+# Folders (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#virtual-
+# folders).
+# The default value is: doc.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_VIRTUAL_FOLDER = doc
+
+# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom
+# filter to add. For more information please see Qt Help Project / Custom
+# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
+# filters).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_CUST_FILTER_NAME =
+
+# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the
+# custom filter to add. For more information please see Qt Help Project / Custom
+# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
+# filters).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_CUST_FILTER_ATTRS =
+
+# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this
+# project's filter section matches. Qt Help Project / Filter Attributes (see:
+# http://qt-project.org/doc/qt-4.8/qthelpproject.html#filter-attributes).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_SECT_FILTER_ATTRS =
+
+# The QHG_LOCATION tag can be used to specify the location of Qt's
+# qhelpgenerator. If non-empty doxygen will try to run qhelpgenerator on the
+# generated .qhp file.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHG_LOCATION =
+
+# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be
+# generated, together with the HTML files, they form an Eclipse help plugin. To
+# install this plugin and make it available under the help contents menu in
+# Eclipse, the contents of the directory containing the HTML and XML files needs
+# to be copied into the plugins directory of eclipse. The name of the directory
+# within the plugins directory should be the same as the ECLIPSE_DOC_ID value.
+# After copying Eclipse needs to be restarted before the help appears.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_ECLIPSEHELP = NO
+
+# A unique identifier for the Eclipse help plugin. When installing the plugin
+# the directory name containing the HTML and XML files should also have this
+# name. Each documentation set should have its own identifier.
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES.
+
+ECLIPSE_DOC_ID = org.doxygen.Project
+
+# If you want full control over the layout of the generated HTML pages it might
+# be necessary to disable the index and replace it with your own. The
+# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top
+# of each HTML page. A value of NO enables the index and the value YES disables
+# it. Since the tabs in the index contain the same information as the navigation
+# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+DISABLE_INDEX = NO
+
+# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index
+# structure should be generated to display hierarchical information. If the tag
+# value is set to YES, a side panel will be generated containing a tree-like
+# index structure (just like the one that is generated for HTML Help). For this
+# to work a browser that supports JavaScript, DHTML, CSS and frames is required
+# (i.e. any modern browser). Windows users are probably better off using the
+# HTML help feature. Via custom stylesheets (see HTML_EXTRA_STYLESHEET) one can
+# further fine-tune the look of the index. As an example, the default style
+# sheet generated by doxygen has an example that shows how to put an image at
+# the root of the tree instead of the PROJECT_NAME. Since the tree basically has
+# the same information as the tab index, you could consider setting
+# DISABLE_INDEX to YES when enabling this option.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_TREEVIEW = YES
+
+# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that
+# doxygen will group on one line in the generated HTML documentation.
+#
+# Note that a value of 0 will completely suppress the enum values from appearing
+# in the overview section.
+# Minimum value: 0, maximum value: 20, default value: 4.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+ENUM_VALUES_PER_LINE = 4
+
+# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used
+# to set the initial width (in pixels) of the frame in which the tree is shown.
+# Minimum value: 0, maximum value: 1500, default value: 250.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+TREEVIEW_WIDTH = 250
+
+# When the EXT_LINKS_IN_WINDOW option is set to YES doxygen will open links to
+# external symbols imported via tag files in a separate window.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+EXT_LINKS_IN_WINDOW = NO
+
+# Use this tag to change the font size of LaTeX formulas included as images in
+# the HTML documentation. When you change the font size after a successful
+# doxygen run you need to manually remove any form_*.png images from the HTML
+# output directory to force them to be regenerated.
+# Minimum value: 8, maximum value: 50, default value: 10.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+FORMULA_FONTSIZE = 10
+
+# Use the FORMULA_TRANPARENT tag to determine whether or not the images
+# generated for formulas are transparent PNGs. Transparent PNGs are not
+# supported properly for IE 6.0, but are supported on all modern browsers.
+#
+# Note that when changing this option you need to delete any form_*.png files in
+# the HTML output directory before the changes have effect.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+FORMULA_TRANSPARENT = YES
+
+# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see
+# http://www.mathjax.org) which uses client side Javascript for the rendering
+# instead of using prerendered bitmaps. Use this if you do not have LaTeX
+# installed or if you want to formulas look prettier in the HTML output. When
+# enabled you may also need to install MathJax separately and configure the path
+# to it using the MATHJAX_RELPATH option.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+USE_MATHJAX = NO
+
+# When MathJax is enabled you can set the default output format to be used for
+# the MathJax output. See the MathJax site (see:
+# http://docs.mathjax.org/en/latest/output.html) for more details.
+# Possible values are: HTML-CSS (which is slower, but has the best
+# compatibility), NativeMML (i.e. MathML) and SVG.
+# The default value is: HTML-CSS.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_FORMAT = HTML-CSS
+
+# When MathJax is enabled you need to specify the location relative to the HTML
+# output directory using the MATHJAX_RELPATH option. The destination directory
+# should contain the MathJax.js script. For instance, if the mathjax directory
+# is located at the same level as the HTML output directory, then
+# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax
+# Content Delivery Network so you can quickly see the result without installing
+# MathJax. However, it is strongly recommended to install a local copy of
+# MathJax from http://www.mathjax.org before deployment.
+# The default value is: http://cdn.mathjax.org/mathjax/latest.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest
+
+# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax
+# extension names that should be enabled during MathJax rendering. For example
+# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_EXTENSIONS =
+
+# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces
+# of code that will be used on startup of the MathJax code. See the MathJax site
+# (see: http://docs.mathjax.org/en/latest/output.html) for more details. For an
+# example see the documentation.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_CODEFILE =
+
+# When the SEARCHENGINE tag is enabled doxygen will generate a search box for
+# the HTML output. The underlying search engine uses javascript and DHTML and
+# should work on any modern browser. Note that when using HTML help
+# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET)
+# there is already a search function so this one should typically be disabled.
+# For large projects the javascript based search engine can be slow, then
+# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to
+# search using the keyboard; to jump to the search box use <access key> + S
+# (what the <access key> is depends on the OS and browser, but it is typically
+# <CTRL>, <ALT>/<option>, or both). Inside the search box use the <cursor down
+# key> to jump into the search results window, the results can be navigated
+# using the <cursor keys>. Press <Enter> to select an item or <escape> to cancel
+# the search. The filter options can be selected when the cursor is inside the
+# search box by pressing <Shift>+<cursor down>. Also here use the <cursor keys>
+# to select a filter and <Enter> or <escape> to activate or cancel the filter
+# option.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+SEARCHENGINE = YES
+
+# When the SERVER_BASED_SEARCH tag is enabled the search engine will be
+# implemented using a web server instead of a web client using Javascript. There
+# are two flavors of web server based searching depending on the EXTERNAL_SEARCH
+# setting. When disabled, doxygen will generate a PHP script for searching and
+# an index file used by the script. When EXTERNAL_SEARCH is enabled the indexing
+# and searching needs to be provided by external tools. See the section
+# "External Indexing and Searching" for details.
+# The default value is: NO.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SERVER_BASED_SEARCH = NO
+
+# When EXTERNAL_SEARCH tag is enabled doxygen will no longer generate the PHP
+# script for searching. Instead the search results are written to an XML file
+# which needs to be processed by an external indexer. Doxygen will invoke an
+# external search engine pointed to by the SEARCHENGINE_URL option to obtain the
+# search results.
+#
+# Doxygen ships with an example indexer ( doxyindexer) and search engine
+# (doxysearch.cgi) which are based on the open source search engine library
+# Xapian (see: http://xapian.org/).
+#
+# See the section "External Indexing and Searching" for details.
+# The default value is: NO.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTERNAL_SEARCH = NO
+
+# The SEARCHENGINE_URL should point to a search engine hosted by a web server
+# which will return the search results when EXTERNAL_SEARCH is enabled.
+#
+# Doxygen ships with an example indexer ( doxyindexer) and search engine
+# (doxysearch.cgi) which are based on the open source search engine library
+# Xapian (see: http://xapian.org/). See the section "External Indexing and
+# Searching" for details.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SEARCHENGINE_URL =
+
+# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the unindexed
+# search data is written to a file for indexing by an external tool. With the
+# SEARCHDATA_FILE tag the name of this file can be specified.
+# The default file is: searchdata.xml.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SEARCHDATA_FILE = searchdata.xml
+
+# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the
+# EXTERNAL_SEARCH_ID tag can be used as an identifier for the project. This is
+# useful in combination with EXTRA_SEARCH_MAPPINGS to search through multiple
+# projects and redirect the results back to the right project.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTERNAL_SEARCH_ID =
+
+# The EXTRA_SEARCH_MAPPINGS tag can be used to enable searching through doxygen
+# projects other than the one defined by this configuration file, but that are
+# all added to the same external search index. Each project needs to have a
+# unique id set via EXTERNAL_SEARCH_ID. The search mapping then maps the id of
+# to a relative location where the documentation can be found. The format is:
+# EXTRA_SEARCH_MAPPINGS = tagname1=loc1 tagname2=loc2 ...
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTRA_SEARCH_MAPPINGS =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the LaTeX output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_LATEX tag is set to YES doxygen will generate LaTeX output.
+# The default value is: YES.
+
+GENERATE_LATEX = NO
+
+# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: latex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_OUTPUT = latex
+
+# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be
+# invoked.
+#
+# Note that when enabling USE_PDFLATEX this option is only used for generating
+# bitmaps for formulas in the HTML output, but not in the Makefile that is
+# written to the output directory.
+# The default file is: latex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_CMD_NAME = latex
+
+# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to generate
+# index for LaTeX.
+# The default file is: makeindex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+MAKEINDEX_CMD_NAME = makeindex
+
+# If the COMPACT_LATEX tag is set to YES doxygen generates more compact LaTeX
+# documents. This may be useful for small projects and may help to save some
+# trees in general.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+COMPACT_LATEX = NO
+
+# The PAPER_TYPE tag can be used to set the paper type that is used by the
+# printer.
+# Possible values are: a4 (210 x 297 mm), letter (8.5 x 11 inches), legal (8.5 x
+# 14 inches) and executive (7.25 x 10.5 inches).
+# The default value is: a4.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+PAPER_TYPE = a4
+
+# The EXTRA_PACKAGES tag can be used to specify one or more LaTeX package names
+# that should be included in the LaTeX output. To get the times font for
+# instance you can specify
+# EXTRA_PACKAGES=times
+# If left blank no extra packages will be included.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+EXTRA_PACKAGES = amsmath
+
+# The LATEX_HEADER tag can be used to specify a personal LaTeX header for the
+# generated LaTeX document. The header should contain everything until the first
+# chapter. If it is left blank doxygen will generate a standard header. See
+# section "Doxygen usage" for information on how to let doxygen write the
+# default header to a separate file.
+#
+# Note: Only use a user-defined header if you know what you are doing! The
+# following commands have a special meaning inside the header: $title,
+# $datetime, $date, $doxygenversion, $projectname, $projectnumber. Doxygen will
+# replace them by respectively the title of the page, the current date and time,
+# only the current date, the version number of doxygen, the project name (see
+# PROJECT_NAME), or the project number (see PROJECT_NUMBER).
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_HEADER =
+
+# The LATEX_FOOTER tag can be used to specify a personal LaTeX footer for the
+# generated LaTeX document. The footer should contain everything after the last
+# chapter. If it is left blank doxygen will generate a standard footer.
+#
+# Note: Only use a user-defined footer if you know what you are doing!
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_FOOTER =
+
+# The LATEX_EXTRA_FILES tag can be used to specify one or more extra images or
+# other source files which should be copied to the LATEX_OUTPUT output
+# directory. Note that the files will be copied as-is; there are no commands or
+# markers available.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_EXTRA_FILES =
+
+# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated is
+# prepared for conversion to PDF (using ps2pdf or pdflatex). The PDF file will
+# contain links (just like the HTML output) instead of page references. This
+# makes the output suitable for online browsing using a PDF viewer.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+PDF_HYPERLINKS = YES
+
+# If the LATEX_PDFLATEX tag is set to YES, doxygen will use pdflatex to generate
+# the PDF file directly from the LaTeX files. Set this option to YES to get a
+# higher quality PDF documentation.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+USE_PDFLATEX = YES
+
+# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \batchmode
+# command to the generated LaTeX files. This will instruct LaTeX to keep running
+# if errors occur, instead of asking the user for help. This option is also used
+# when generating formulas in HTML.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_BATCHMODE = NO
+
+# If the LATEX_HIDE_INDICES tag is set to YES then doxygen will not include the
+# index chapters (such as File Index, Compound Index, etc.) in the output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_HIDE_INDICES = NO
+
+# If the LATEX_SOURCE_CODE tag is set to YES then doxygen will include source
+# code with syntax highlighting in the LaTeX output.
+#
+# Note that which sources are shown also depends on other settings such as
+# SOURCE_BROWSER.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_SOURCE_CODE = NO
+
+# The LATEX_BIB_STYLE tag can be used to specify the style to use for the
+# bibliography, e.g. plainnat, or ieeetr. See
+# http://en.wikipedia.org/wiki/BibTeX and \cite for more info.
+# The default value is: plain.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_BIB_STYLE = plain
+
+#---------------------------------------------------------------------------
+# Configuration options related to the RTF output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_RTF tag is set to YES doxygen will generate RTF output. The
+# RTF output is optimized for Word 97 and may not look too pretty with other RTF
+# readers/editors.
+# The default value is: NO.
+
+GENERATE_RTF = NO
+
+# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: rtf.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_OUTPUT = rtf
+
+# If the COMPACT_RTF tag is set to YES doxygen generates more compact RTF
+# documents. This may be useful for small projects and may help to save some
+# trees in general.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+COMPACT_RTF = NO
+
+# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated will
+# contain hyperlink fields. The RTF file will contain links (just like the HTML
+# output) instead of page references. This makes the output suitable for online
+# browsing using Word or some other Word compatible readers that support those
+# fields.
+#
+# Note: WordPad (write) and others do not support links.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_HYPERLINKS = NO
+
+# Load stylesheet definitions from file. Syntax is similar to doxygen's config
+# file, i.e. a series of assignments. You only have to provide replacements,
+# missing definitions are set to their default value.
+#
+# See also section "Doxygen usage" for information on how to generate the
+# default style sheet that doxygen normally uses.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_STYLESHEET_FILE =
+
+# Set optional variables used in the generation of an RTF document. Syntax is
+# similar to doxygen's config file. A template extensions file can be generated
+# using doxygen -e rtf extensionFile.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_EXTENSIONS_FILE =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the man page output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_MAN tag is set to YES doxygen will generate man pages for
+# classes and files.
+# The default value is: NO.
+
+GENERATE_MAN = NO
+
+# The MAN_OUTPUT tag is used to specify where the man pages will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it. A directory man3 will be created inside the directory specified by
+# MAN_OUTPUT.
+# The default directory is: man.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_OUTPUT = man
+
+# The MAN_EXTENSION tag determines the extension that is added to the generated
+# man pages. In case the manual section does not start with a number, the number
+# 3 is prepended. The dot (.) at the beginning of the MAN_EXTENSION tag is
+# optional.
+# The default value is: .3.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_EXTENSION = .3
+
+# The MAN_SUBDIR tag determines the name of the directory created within
+# MAN_OUTPUT in which the man pages are placed. If defaults to man followed by
+# MAN_EXTENSION with the initial . removed.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_SUBDIR =
+
+# If the MAN_LINKS tag is set to YES and doxygen generates man output, then it
+# will generate one additional man file for each entity documented in the real
+# man page(s). These additional files only source the real man page, but without
+# them the man command would be unable to find the correct page.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_LINKS = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the XML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_XML tag is set to YES doxygen will generate an XML file that
+# captures the structure of the code including all documentation.
+# The default value is: NO.
+
+GENERATE_XML = NO
+
+# The XML_OUTPUT tag is used to specify where the XML pages will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: xml.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_OUTPUT = xml
+
+# If the XML_PROGRAMLISTING tag is set to YES doxygen will dump the program
+# listings (including syntax highlighting and cross-referencing information) to
+# the XML output. Note that enabling this will significantly increase the size
+# of the XML output.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_PROGRAMLISTING = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to the DOCBOOK output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_DOCBOOK tag is set to YES doxygen will generate Docbook files
+# that can be used to generate PDF.
+# The default value is: NO.
+
+GENERATE_DOCBOOK = NO
+
+# The DOCBOOK_OUTPUT tag is used to specify where the Docbook pages will be put.
+# If a relative path is entered the value of OUTPUT_DIRECTORY will be put in
+# front of it.
+# The default directory is: docbook.
+# This tag requires that the tag GENERATE_DOCBOOK is set to YES.
+
+DOCBOOK_OUTPUT = docbook
+
+#---------------------------------------------------------------------------
+# Configuration options for the AutoGen Definitions output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_AUTOGEN_DEF tag is set to YES doxygen will generate an AutoGen
+# Definitions (see http://autogen.sf.net) file that captures the structure of
+# the code including all documentation. Note that this feature is still
+# experimental and incomplete at the moment.
+# The default value is: NO.
+
+GENERATE_AUTOGEN_DEF = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the Perl module output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_PERLMOD tag is set to YES doxygen will generate a Perl module
+# file that captures the structure of the code including all documentation.
+#
+# Note that this feature is still experimental and incomplete at the moment.
+# The default value is: NO.
+
+GENERATE_PERLMOD = NO
+
+# If the PERLMOD_LATEX tag is set to YES doxygen will generate the necessary
+# Makefile rules, Perl scripts and LaTeX code to be able to generate PDF and DVI
+# output from the Perl module output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_LATEX = NO
+
+# If the PERLMOD_PRETTY tag is set to YES the Perl module output will be nicely
+# formatted so it can be parsed by a human reader. This is useful if you want to
+# understand what is going on. On the other hand, if this tag is set to NO the
+# size of the Perl module output will be much smaller and Perl will parse it
+# just the same.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_PRETTY = YES
+
+# The names of the make variables in the generated doxyrules.make file are
+# prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. This is useful
+# so different doxyrules.make files included by the same Makefile don't
+# overwrite each other's variables.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_MAKEVAR_PREFIX =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the preprocessor
+#---------------------------------------------------------------------------
+
+# If the ENABLE_PREPROCESSING tag is set to YES doxygen will evaluate all
+# C-preprocessor directives found in the sources and include files.
+# The default value is: YES.
+
+ENABLE_PREPROCESSING = YES
+
+# If the MACRO_EXPANSION tag is set to YES doxygen will expand all macro names
+# in the source code. If set to NO only conditional compilation will be
+# performed. Macro expansion can be done in a controlled way by setting
+# EXPAND_ONLY_PREDEF to YES.
+# The default value is: NO.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+MACRO_EXPANSION = NO
+
+# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES then
+# the macro expansion is limited to the macros specified with the PREDEFINED and
+# EXPAND_AS_DEFINED tags.
+# The default value is: NO.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+EXPAND_ONLY_PREDEF = NO
+
+# If the SEARCH_INCLUDES tag is set to YES the includes files in the
+# INCLUDE_PATH will be searched if a #include is found.
+# The default value is: YES.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+SEARCH_INCLUDES = YES
+
+# The INCLUDE_PATH tag can be used to specify one or more directories that
+# contain include files that are not input files but should be processed by the
+# preprocessor.
+# This tag requires that the tag SEARCH_INCLUDES is set to YES.
+
+INCLUDE_PATH =
+
+# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard
+# patterns (like *.h and *.hpp) to filter out the header-files in the
+# directories. If left blank, the patterns specified with FILE_PATTERNS will be
+# used.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+INCLUDE_FILE_PATTERNS =
+
+# The PREDEFINED tag can be used to specify one or more macro names that are
+# defined before the preprocessor is started (similar to the -D option of e.g.
+# gcc). The argument of the tag is a list of macros of the form: name or
+# name=definition (no spaces). If the definition and the "=" are omitted, "=1"
+# is assumed. To prevent a macro definition from being undefined via #undef or
+# recursively expanded use the := operator instead of the = operator.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+PREDEFINED = DOXYGEN_SHOULD_SKIP_THIS
+
+# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then this
+# tag can be used to specify a list of macro names that should be expanded. The
+# macro definition that is found in the sources will be used. Use the PREDEFINED
+# tag if you want to use a different macro definition that overrules the
+# definition found in the source code.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+EXPAND_AS_DEFINED =
+
+# If the SKIP_FUNCTION_MACROS tag is set to YES then doxygen's preprocessor will
+# remove all references to function-like macros that are alone on a line, have
+# an all uppercase name, and do not end with a semicolon. Such function macros
+# are typically used for boiler-plate code, and will confuse the parser if not
+# removed.
+# The default value is: YES.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+SKIP_FUNCTION_MACROS = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to external references
+#---------------------------------------------------------------------------
+
+# The TAGFILES tag can be used to specify one or more tag files. For each tag
+# file the location of the external documentation should be added. The format of
+# a tag file without this location is as follows:
+# TAGFILES = file1 file2 ...
+# Adding location for the tag files is done as follows:
+# TAGFILES = file1=loc1 "file2 = loc2" ...
+# where loc1 and loc2 can be relative or absolute paths or URLs. See the
+# section "Linking to external documentation" for more information about the use
+# of tag files.
+# Note: Each tag file must have a unique name (where the name does NOT include
+# the path). If a tag file is not located in the directory in which doxygen is
+# run, you must also specify the path to the tagfile here.
+
+TAGFILES = ../../ct_doc/doc/tags/ct_core.tag=../../../ct_core/doc/html \
+ ../../ct_doc/doc/tags/ct_optcon.tag=../../../ct_optcon/doc/html
+
+# When a file name is specified after GENERATE_TAGFILE, doxygen will create a
+# tag file that is based on the input files it reads. See section "Linking to
+# external documentation" for more information about the usage of tag files.
+
+GENERATE_TAGFILE = ../../ct_doc/doc/tags/ct_rbd.tag
+
+# If the ALLEXTERNALS tag is set to YES all external class will be listed in the
+# class index. If set to NO only the inherited external classes will be listed.
+# The default value is: NO.
+
+ALLEXTERNALS = NO
+
+# If the EXTERNAL_GROUPS tag is set to YES all external groups will be listed in
+# the modules index. If set to NO, only the current project's groups will be
+# listed.
+# The default value is: YES.
+
+EXTERNAL_GROUPS = NO
+
+# If the EXTERNAL_PAGES tag is set to YES all external pages will be listed in
+# the related pages index. If set to NO, only the current project's pages will
+# be listed.
+# The default value is: YES.
+
+EXTERNAL_PAGES = YES
+
+# The PERL_PATH should be the absolute path and name of the perl script
+# interpreter (i.e. the result of 'which perl').
+# The default file (with absolute path) is: /usr/bin/perl.
+
+PERL_PATH = /usr/bin/perl
+
+#---------------------------------------------------------------------------
+# Configuration options related to the dot tool
+#---------------------------------------------------------------------------
+
+# If the CLASS_DIAGRAMS tag is set to YES doxygen will generate a class diagram
+# (in HTML and LaTeX) for classes with base or super classes. Setting the tag to
+# NO turns the diagrams off. Note that this option also works with HAVE_DOT
+# disabled, but it is recommended to install and use dot, since it yields more
+# powerful graphs.
+# The default value is: YES.
+
+CLASS_DIAGRAMS = YES
+
+# You can define message sequence charts within doxygen comments using the \msc
+# command. Doxygen will then run the mscgen tool (see:
+# http://www.mcternan.me.uk/mscgen/)) to produce the chart and insert it in the
+# documentation. The MSCGEN_PATH tag allows you to specify the directory where
+# the mscgen tool resides. If left empty the tool is assumed to be found in the
+# default search path.
+
+MSCGEN_PATH =
+
+# You can include diagrams made with dia in doxygen documentation. Doxygen will
+# then run dia to produce the diagram and insert it in the documentation. The
+# DIA_PATH tag allows you to specify the directory where the dia binary resides.
+# If left empty dia is assumed to be found in the default search path.
+
+DIA_PATH =
+
+# If set to YES, the inheritance and collaboration graphs will hide inheritance
+# and usage relations if the target is undocumented or is not a class.
+# The default value is: YES.
+
+HIDE_UNDOC_RELATIONS = YES
+
+# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is
+# available from the path. This tool is part of Graphviz (see:
+# http://www.graphviz.org/), a graph visualization toolkit from AT&T and Lucent
+# Bell Labs. The other options in this section have no effect if this option is
+# set to NO
+# The default value is: YES.
+
+HAVE_DOT = NO
+
+# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is allowed
+# to run in parallel. When set to 0 doxygen will base this on the number of
+# processors available in the system. You can set it explicitly to a value
+# larger than 0 to get control over the balance between CPU load and processing
+# speed.
+# Minimum value: 0, maximum value: 32, default value: 0.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_NUM_THREADS = 0
+
+# When you want a differently looking font n the dot files that doxygen
+# generates you can specify the font name using DOT_FONTNAME. You need to make
+# sure dot is able to find the font, which can be done by putting it in a
+# standard location or by setting the DOTFONTPATH environment variable or by
+# setting DOT_FONTPATH to the directory containing the font.
+# The default value is: Helvetica.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTNAME = Helvetica
+
+# The DOT_FONTSIZE tag can be used to set the size (in points) of the font of
+# dot graphs.
+# Minimum value: 4, maximum value: 24, default value: 10.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTSIZE = 10
+
+# By default doxygen will tell dot to use the default font as specified with
+# DOT_FONTNAME. If you specify a different font using DOT_FONTNAME you can set
+# the path where dot can find it using this tag.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTPATH =
+
+# If the CLASS_GRAPH tag is set to YES then doxygen will generate a graph for
+# each documented class showing the direct and indirect inheritance relations.
+# Setting this tag to YES will force the CLASS_DIAGRAMS tag to NO.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CLASS_GRAPH = YES
+
+# If the COLLABORATION_GRAPH tag is set to YES then doxygen will generate a
+# graph for each documented class showing the direct and indirect implementation
+# dependencies (inheritance, containment, and class references variables) of the
+# class with other documented classes.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+COLLABORATION_GRAPH = YES
+
+# If the GROUP_GRAPHS tag is set to YES then doxygen will generate a graph for
+# groups, showing the direct groups dependencies.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GROUP_GRAPHS = YES
+
+# If the UML_LOOK tag is set to YES doxygen will generate inheritance and
+# collaboration diagrams in a style similar to the OMG's Unified Modeling
+# Language.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+UML_LOOK = NO
+
+# If the UML_LOOK tag is enabled, the fields and methods are shown inside the
+# class node. If there are many fields or methods and many nodes the graph may
+# become too big to be useful. The UML_LIMIT_NUM_FIELDS threshold limits the
+# number of items for each type to make the size more manageable. Set this to 0
+# for no limit. Note that the threshold may be exceeded by 50% before the limit
+# is enforced. So when you set the threshold to 10, up to 15 fields may appear,
+# but if the number exceeds 15, the total amount of fields shown is limited to
+# 10.
+# Minimum value: 0, maximum value: 100, default value: 10.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+UML_LIMIT_NUM_FIELDS = 10
+
+# If the TEMPLATE_RELATIONS tag is set to YES then the inheritance and
+# collaboration graphs will show the relations between templates and their
+# instances.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+TEMPLATE_RELATIONS = NO
+
+# If the INCLUDE_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are set to
+# YES then doxygen will generate a graph for each documented file showing the
+# direct and indirect include dependencies of the file with other documented
+# files.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INCLUDE_GRAPH = YES
+
+# If the INCLUDED_BY_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are
+# set to YES then doxygen will generate a graph for each documented file showing
+# the direct and indirect include dependencies of the file with other documented
+# files.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INCLUDED_BY_GRAPH = YES
+
+# If the CALL_GRAPH tag is set to YES then doxygen will generate a call
+# dependency graph for every global function or class method.
+#
+# Note that enabling this option will significantly increase the time of a run.
+# So in most cases it will be better to enable call graphs for selected
+# functions only using the \callgraph command.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CALL_GRAPH = NO
+
+# If the CALLER_GRAPH tag is set to YES then doxygen will generate a caller
+# dependency graph for every global function or class method.
+#
+# Note that enabling this option will significantly increase the time of a run.
+# So in most cases it will be better to enable caller graphs for selected
+# functions only using the \callergraph command.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CALLER_GRAPH = NO
+
+# If the GRAPHICAL_HIERARCHY tag is set to YES then doxygen will graphical
+# hierarchy of all classes instead of a textual one.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GRAPHICAL_HIERARCHY = YES
+
+# If the DIRECTORY_GRAPH tag is set to YES then doxygen will show the
+# dependencies a directory has on other directories in a graphical way. The
+# dependency relations are determined by the #include relations between the
+# files in the directories.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DIRECTORY_GRAPH = YES
+
+# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images
+# generated by dot.
+# Note: If you choose svg you need to set HTML_FILE_EXTENSION to xhtml in order
+# to make the SVG files visible in IE 9+ (other browsers do not have this
+# requirement).
+# Possible values are: png, png:cairo, png:cairo:cairo, png:cairo:gd, png:gd,
+# png:gd:gd, jpg, jpg:cairo, jpg:cairo:gd, jpg:gd, jpg:gd:gd, gif, gif:cairo,
+# gif:cairo:gd, gif:gd, gif:gd:gd and svg.
+# The default value is: png.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_IMAGE_FORMAT = png
+
+# If DOT_IMAGE_FORMAT is set to svg, then this option can be set to YES to
+# enable generation of interactive SVG images that allow zooming and panning.
+#
+# Note that this requires a modern browser other than Internet Explorer. Tested
+# and working are Firefox, Chrome, Safari, and Opera.
+# Note: For IE 9+ you need to set HTML_FILE_EXTENSION to xhtml in order to make
+# the SVG files visible. Older versions of IE do not have SVG support.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INTERACTIVE_SVG = NO
+
+# The DOT_PATH tag can be used to specify the path where the dot tool can be
+# found. If left blank, it is assumed the dot tool can be found in the path.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_PATH =
+
+# The DOTFILE_DIRS tag can be used to specify one or more directories that
+# contain dot files that are included in the documentation (see the \dotfile
+# command).
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOTFILE_DIRS =
+
+# The MSCFILE_DIRS tag can be used to specify one or more directories that
+# contain msc files that are included in the documentation (see the \mscfile
+# command).
+
+MSCFILE_DIRS =
+
+# The DIAFILE_DIRS tag can be used to specify one or more directories that
+# contain dia files that are included in the documentation (see the \diafile
+# command).
+
+DIAFILE_DIRS =
+
+# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of nodes
+# that will be shown in the graph. If the number of nodes in a graph becomes
+# larger than this value, doxygen will truncate the graph, which is visualized
+# by representing a node as a red box. Note that doxygen if the number of direct
+# children of the root node in a graph is already larger than
+# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note that
+# the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH.
+# Minimum value: 0, maximum value: 10000, default value: 50.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_GRAPH_MAX_NODES = 50
+
+# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the graphs
+# generated by dot. A depth value of 3 means that only nodes reachable from the
+# root by following a path via at most 3 edges will be shown. Nodes that lay
+# further from the root node will be omitted. Note that setting this option to 1
+# or 2 may greatly reduce the computation time needed for large code bases. Also
+# note that the size of a graph can be further restricted by
+# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction.
+# Minimum value: 0, maximum value: 1000, default value: 0.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+MAX_DOT_GRAPH_DEPTH = 0
+
+# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent
+# background. This is disabled by default, because dot on Windows does not seem
+# to support this out of the box.
+#
+# Warning: Depending on the platform used, enabling this option may lead to
+# badly anti-aliased labels on the edges of a graph (i.e. they become hard to
+# read).
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_TRANSPARENT = NO
+
+# Set the DOT_MULTI_TARGETS tag to YES allow dot to generate multiple output
+# files in one run (i.e. multiple -o and -T options on the command line). This
+# makes dot run faster, but since only newer versions of dot (>1.8.10) support
+# this, this feature is disabled by default.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_MULTI_TARGETS = YES
+
+# If the GENERATE_LEGEND tag is set to YES doxygen will generate a legend page
+# explaining the meaning of the various boxes and arrows in the dot generated
+# graphs.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GENERATE_LEGEND = YES
+
+# If the DOT_CLEANUP tag is set to YES doxygen will remove the intermediate dot
+# files that are used to generate the various graphs.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_CLEANUP = YES
diff --git a/ct_rbd/doc/mainpage.dox b/ct_rbd/doc/mainpage.dox
new file mode 100644
index 0000000..1f0eb5b
--- /dev/null
+++ b/ct_rbd/doc/mainpage.dox
@@ -0,0 +1,14 @@
+/*!
+\mainpage CT Rigid Body Dynamics
+
+\section ct_rbd_module Control Toolbox - Rigid Body Dynamics Module
+
+This module is part of the ADRL Control Toolbox ('CT'), an open-source C++ library for efficient modelling, control,
+estimation and trajectory optimization for robotics.
+
+The Rigid Body Dynamics (RBD) Module contains helpers for kinematic and dynamic computations for modelling multi body systems.
+
+\section ct_rbd_install Installation and Usage
+CT Rigid Body Dynamics is part of the Control Toolbox suite. See the <a href="../../../ct_doc/doc/html/index.html">CT main documentation</a> for installation and usage instructions.
+
+*/
diff --git a/ct_rbd/include/ct/rbd/Common b/ct_rbd/include/ct/rbd/Common
new file mode 100644
index 0000000..72c27a0
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/Common
@@ -0,0 +1,9 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "common/SpatialForceVector.h"
diff --git a/ct_rbd/include/ct/rbd/Common-impl b/ct_rbd/include/ct/rbd/Common-impl
new file mode 100644
index 0000000..0d1176c
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/Common-impl
@@ -0,0 +1,10 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+// put implementation files here
\ No newline at end of file
diff --git a/ct_rbd/include/ct/rbd/Internal b/ct_rbd/include/ct/rbd/Internal
new file mode 100644
index 0000000..fce72b2
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/Internal
@@ -0,0 +1,11 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+#include "internal/TraitSelectorSpecs.h"
+
diff --git a/ct_rbd/include/ct/rbd/Internal-impl b/ct_rbd/include/ct/rbd/Internal-impl
new file mode 100644
index 0000000..89fedc6
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/Internal-impl
@@ -0,0 +1,10 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+// put implementation files here
diff --git a/ct_rbd/include/ct/rbd/Nloc b/ct_rbd/include/ct/rbd/Nloc
new file mode 100644
index 0000000..833aeea
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/Nloc
@@ -0,0 +1,11 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+#include "nloc/FixBaseNLOC.h"
+#include "nloc/FloatingBaseNLOCContactModel.h"
diff --git a/ct_rbd/include/ct/rbd/Nloc-impl b/ct_rbd/include/ct/rbd/Nloc-impl
new file mode 100644
index 0000000..0db5b74
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/Nloc-impl
@@ -0,0 +1,11 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+#include "nloc/FixBaseNLOC-impl.h"
+#include "nloc/FloatingBaseNLOCContactModel-impl.h"
\ No newline at end of file
diff --git a/ct_rbd/include/ct/rbd/Robot b/ct_rbd/include/ct/rbd/Robot
new file mode 100644
index 0000000..1a08405
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/Robot
@@ -0,0 +1,26 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+#include "robot/RobCoGenContainer.h"
+#include "robot/Kinematics.h"
+#include "robot/Dynamics.h"
+
+#include "robot/actuator/SecondOrderActuatorDynamics.h"
+#include "robot/actuator/SEADynamicsFirstOrder.h"
+
+#include "robot/control/IDControllerFB.h"
+#include "robot/control/WholeBodyController.h"
+#include "robot/control/InfiniteHorizonLQRwithInverseDynamics.h"
+#include "robot/control/JointPositionPIDController.h"
+#include "robot/control/SelectionMatrix.h"
+
+#include "robot/costfunction/TermTaskspacePosition.hpp"
+#include "robot/costfunction/TermTaskspacePose.hpp"
+
+#include "robot/kinematics/EndEffector.h"
diff --git a/ct_rbd/include/ct/rbd/Robot-impl b/ct_rbd/include/ct/rbd/Robot-impl
new file mode 100644
index 0000000..7f7df86
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/Robot-impl
@@ -0,0 +1,17 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+#include "robot/actuator/SecondOrderActuatorDynamics-impl.h"
+#include "robot/actuator/SEADynamicsFirstOrder-impl.h"
+
+#include "robot/control/JointPositionPIDController-impl.h"
+#include "robot/control/SelectionMatrix-impl.h"
+#include "robot/control/WholeBodyController-impl.h"
+
+#include "robot/kinematics/EndEffector-impl.h"
diff --git a/ct_rbd/include/ct/rbd/State b/ct_rbd/include/ct/rbd/State
new file mode 100644
index 0000000..76a6cf8
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/State
@@ -0,0 +1,10 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+#include "state/RBDState.h"
diff --git a/ct_rbd/include/ct/rbd/State-impl b/ct_rbd/include/ct/rbd/State-impl
new file mode 100644
index 0000000..a6d6536
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/State-impl
@@ -0,0 +1,13 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#ifndef INCLUDE_CT_RBD_STATE_IMPL_
+#define INCLUDE_CT_RBD_STATE_IMPL_
+
+// put implementation files here
+
+#endif /* INCLUDE_CT_RBD_STATE_IMPL_ */
diff --git a/ct_rbd/include/ct/rbd/Systems b/ct_rbd/include/ct/rbd/Systems
new file mode 100644
index 0000000..fedf0a2
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/Systems
@@ -0,0 +1,15 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+#include "systems/FixBaseFDSystem.h"
+#include "systems/FixBaseFDSystemSymplectic.h"
+#include "systems/FloatingBaseFDSystem.h"
+#include "systems/ProjectedFDSystem.h"
+
+#include "systems/linear/RbdLinearizer.h"
diff --git a/ct_rbd/include/ct/rbd/Systems-impl b/ct_rbd/include/ct/rbd/Systems-impl
new file mode 100644
index 0000000..89fedc6
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/Systems-impl
@@ -0,0 +1,10 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+// put implementation files here
diff --git a/ct_rbd/include/ct/rbd/common/SpatialForceVector.h b/ct_rbd/include/ct/rbd/common/SpatialForceVector.h
new file mode 100644
index 0000000..69c4610
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/common/SpatialForceVector.h
@@ -0,0 +1,117 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#ifndef INCLUDE_CT_RBD_ROBOT_KINEMATICS_EEFORCE_H_
+#define INCLUDE_CT_RBD_ROBOT_KINEMATICS_EEFORCE_H_
+
+namespace ct {
+namespace rbd {
+
+/*!
+ * \brief A spatial force vector
+ * This vector contains a torque (angular) in the upper three rows
+ * and a linear force in the lower three rows
+ */
+template <typename SCALAR = double>
+class SpatialForceVector : public Eigen::Matrix<SCALAR, 6, 1>
+{
+public:
+ typedef Eigen::Matrix<SCALAR, 6, 1> spatial_force_vector_t; //!< special vector type
+ typedef Eigen::VectorBlock<spatial_force_vector_t, 3>
+ ForceTorqueBlock; //!< 3D force or torque block inside of the spacial vector
+ typedef Eigen::VectorBlock<const spatial_force_vector_t, 3>
+ ForceTorqueBlockConst; //!< const 3D force or torque block inside of the spacial vector
+
+ /*!
+ * \brief Default constructor
+ */
+ SpatialForceVector() {}
+ /*!
+ * \brief Copy constructor
+ */
+ SpatialForceVector(const spatial_force_vector_t& vector6) : spatial_force_vector_t(vector6) {}
+ /*!
+ * \brief Returns the Eigen implementation
+ */
+ spatial_force_vector_t& toImplementation() { return *this; }
+ /*!
+ * \brief Returns the Eigen implementation
+ */
+ const spatial_force_vector_t& toImplementation() const { return *this; }
+ /*!
+ * \brief Sets all entries to zero
+ */
+ SpatialForceVector& setZero()
+ {
+ // for auto diff compatability
+ SCALAR zero(0.0);
+ this->setConstant(zero);
+ return *this;
+ }
+
+ /*!
+ * \brief Comparison operator (exact, subject to floating point rounding)
+ */
+ inline bool operator==(const spatial_force_vector_t& rhs) const { return Base::operator==(rhs); }
+ /*!
+ * \brief Comparison operator (exact, subject to floating point rounding)
+ */
+ inline bool operator!=(const spatial_force_vector_t& rhs) const { return Base::operator!=(rhs); }
+ /*!
+ * \brief Sums two spatial forces component-wise
+ */
+ inline spatial_force_vector_t operator+(const spatial_force_vector_t& rhs) const
+ {
+ return spatial_force_vector_t(Base::operator+(rhs));
+ }
+ /*!
+ * \brief Substracts two spatial forces component-wise
+ */
+ inline spatial_force_vector_t operator-(const spatial_force_vector_t& rhs) const
+ {
+ return spatial_force_vector_t(Base::operator-(rhs));
+ }
+
+ /*!
+ * \brief Scales a spatial forces component-wise
+ */
+ inline spatial_force_vector_t operator*(const SCALAR& scalar) const
+ {
+ return spatial_force_vector_t(Base::operator*(scalar));
+ }
+
+ /*!
+ * \brief Divides a spatial forces component-wise
+ */
+ inline spatial_force_vector_t operator/(const SCALAR& scalar) const
+ {
+ return spatial_force_vector_t(Base::operator/(scalar));
+ }
+
+ /*!
+ * \brief Get the torque block (upper 3D block)
+ */
+ ForceTorqueBlock torque() { return this->template head<3>(); }
+ /*!
+ * \brief Get the torque block (upper 3D block)
+ */
+ const ForceTorqueBlockConst torque() const { return this->template head<3>(); }
+ /*!
+ * \brief Get the force block (lower 3D block)
+ */
+ ForceTorqueBlock force() { return this->template tail<3>(); }
+ /*!
+ * \brief Get the force block (lower 3D block)
+ */
+ const ForceTorqueBlockConst force() const { return this->template tail<3>(); }
+private:
+ typedef Eigen::Matrix<SCALAR, 6, 1> Base;
+};
+}
+}
+
+
+#endif /* INCLUDE_CT_RBD_ROBOT_KINEMATICS_EEFORCE_H_ */
diff --git a/ct_rbd/include/ct/rbd/common/math/JacobiSingularity.h b/ct_rbd/include/ct/rbd/common/math/JacobiSingularity.h
new file mode 100644
index 0000000..ca48764
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/common/math/JacobiSingularity.h
@@ -0,0 +1,89 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace rbd {
+
+/*!
+ * \brief A class computing codition numbers and singular velues of a Jacobian
+ *
+ * \tparam ROWS number of rows
+ * \tparam CLOS number of colums
+ */
+template <size_t ROWS, size_t COLS>
+class JacobiSingularity
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<double, ROWS, COLS> Jacobian;
+ typedef Eigen::Matrix<double, COLS, ROWS> PseudoInverse;
+ typedef Eigen::Matrix<double, ROWS, 1> SingularValues;
+ typedef Eigen::Matrix<double, ROWS, 1> EndeffectorVelocities;
+ typedef Eigen::Matrix<double, COLS, 1> JointVelocities;
+
+ enum ManipulabilityMethod
+ {
+ DIRECT,
+ SINGULAR_VALUES
+ };
+
+public:
+ JacobiSingularity(const Jacobian& J);
+
+ /**
+ * The condition number compares the lowest to the highest eigenvalue (for
+ * square matrix) or singular value (for others).
+ *
+ * High values of ev_max/ev_min imply that some inputs ("null-space") are mapped to
+ * values close to zero outputs, e.g. some actions in operational space are restricted.
+ */
+ double calc_condition_number() const;
+
+ /**
+ * The manipulability describes the freedom of the endeffector for a given
+ * configuration. \f$ w(q) = \sqrt(det(J*J')) \f$
+ *
+ * @param method can be calculated directly as described in Siciliano p. 153
+ * or multiplying the singular values of the Jacobian
+ * @return the area spanned by all possible ee-velocities.
+ */
+ double calc_manipulability(ManipulabilityMethod method = DIRECT) const;
+
+ /**
+ * Singular values are used if eigenvalues are not available, as in non-
+ * square matrices. With Single Value Decomposition (SVD) \f$ J = U S V \f$.
+ * See Siciliano p. 577
+ * @return the diagonals in S
+ */
+ SingularValues calc_singular_values() const;
+
+ /**
+ * Calculates the joint velocity to obtain a specified endeffector velocity.
+ *
+ * In general the Jacobian is not a square matrix, so the inverse is not
+ * defined since there exist either no solution (skinny Matrix) or infinitely
+ * many (fat Matrix). This function computes the pseudo inverse
+ * \f$ J^\dagger = \f$J' (J J')^{-1} \f$ which chooses the "minimum norm" solution out of the
+ * pool of infinitely many solutions. (see Siciliano p. 125).
+ *
+ * @param des_ee_vel desired endeffector velocity
+ * @return qd = J† * des_ee_vel
+ */
+ JointVelocities calc_joint_vel(const EndeffectorVelocities& des_ee_vel) const;
+
+private:
+ Jacobian J_;
+};
+
+
+} // namespace rbd
+} // namespace ct
+
+
+#include "implementation/JacobiSingularity.h"
diff --git a/ct_rbd/include/ct/rbd/common/math/implementation/JacobiSingularity.h b/ct_rbd/include/ct/rbd/common/math/implementation/JacobiSingularity.h
new file mode 100644
index 0000000..225fc25
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/common/math/implementation/JacobiSingularity.h
@@ -0,0 +1,80 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+namespace ct {
+namespace rbd {
+
+template <size_t ROWS, size_t COLS>
+JacobiSingularity<ROWS, COLS>::JacobiSingularity(const Jacobian& J)
+{
+ J_ = J;
+}
+
+
+template <size_t ROWS, size_t COLS>
+double JacobiSingularity<ROWS, COLS>::calc_condition_number() const
+{
+ SingularValues sv = calc_singular_values();
+ return sv.maxCoeff() / sv.minCoeff();
+}
+
+
+template <size_t ROWS, size_t COLS>
+double JacobiSingularity<ROWS, COLS>::calc_manipulability(ManipulabilityMethod method) const
+{
+ double w = 1;
+
+ switch (method)
+ {
+ case DIRECT:
+ {
+ // see Siciliano p. 153
+ w = std::sqrt((J_ * J_.transpose()).determinant());
+ break;
+ }
+ case SINGULAR_VALUES:
+ {
+ // can also be calculated by multiplying the singular values of J
+ SingularValues sv = calc_singular_values();
+ for (int i = 0; i < sv.size(); ++i)
+ w *= sv(i);
+ break;
+ }
+ }
+ return w;
+}
+
+
+template <size_t ROWS, size_t COLS>
+typename JacobiSingularity<ROWS, COLS>::SingularValues JacobiSingularity<ROWS, COLS>::calc_singular_values() const
+{
+ // matrix is not neccesarily square, so eigenvalues do not exist -> use singular values instead
+ Eigen::JacobiSVD<Eigen::MatrixXd> svd(J_, Eigen::ComputeThinU | Eigen::ComputeThinV);
+ return svd.singularValues();
+}
+
+
+template <size_t ROWS, size_t COLS>
+typename JacobiSingularity<ROWS, COLS>::JointVelocities JacobiSingularity<ROWS, COLS>::calc_joint_vel(
+ const EndeffectorVelocities& des_ee_vel) const
+{
+ // Use SVD to compute Pseudo Inverse of J = J'(JJ')^-1 = VSU*
+ Eigen::JacobiSVD<Eigen::MatrixXd> svd(J_, Eigen::ComputeThinU | Eigen::ComputeThinV);
+ JointVelocities qd_des = svd.solve(des_ee_vel);
+
+ // just for comparison, see if same solution can be reached
+ PseudoInverse J_inv = J_.transpose() * (J_ * J_.transpose()).inverse();
+ JointVelocities qd_des_pseudo = J_inv * des_ee_vel;
+
+ //assert(qd_des == qd_des_pseudo);
+
+ return qd_des;
+}
+
+
+} // namespace rbd
+} // namespace ct
diff --git a/ct_rbd/include/ct/rbd/internal/CppADCodegenTrait.h b/ct_rbd/include/ct/rbd/internal/CppADCodegenTrait.h
new file mode 100644
index 0000000..d475f3d
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/internal/CppADCodegenTrait.h
@@ -0,0 +1,98 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <cppad/cg.hpp>
+#include <cppad/cppad.hpp>
+#include <cppad/example/cppad_eigen.hpp>
+
+
+namespace ct {
+namespace rbd {
+namespace internal {
+
+/*!
+ * \brief Trait providing basic mathematical operations for CppAD Codegeneration datatypes
+ */
+class CppADCodegenTrait
+{
+public:
+ typedef CppAD::AD<CppAD::cg::CG<double>> Scalar;
+
+ inline static Scalar sin(const Scalar& x) { return CppAD::sin(x); }
+ inline static Scalar cos(const Scalar& x) { return CppAD::cos(x); }
+ inline static Scalar tan(const Scalar& x) { return CppAD::tan(x); }
+ inline static Scalar sinh(const Scalar& x) { return CppAD::sinh(x); }
+ inline static Scalar cosh(const Scalar& x) { return CppAD::cosh(x); }
+ inline static Scalar tanh(const Scalar& x) { return CppAD::tanh(x); }
+ inline static Scalar exp(const Scalar& x) { return CppAD::exp(x); }
+ inline static Scalar fabs(const Scalar& x) { return CppAD::fabs(x); }
+ template <int Dims>
+ inline static Eigen::Matrix<Scalar, Dims, 1> solve(const Eigen::Matrix<Scalar, Dims, Dims>& A,
+ const Eigen::Matrix<Scalar, Dims, 1>& b)
+ {
+ Eigen::Matrix<Scalar, Dims, Dims> LU;
+ Crout(A, LU);
+
+ Eigen::Matrix<Scalar, Dims, 1> out;
+ solveCrout(LU, b, out);
+
+ return out;
+ }
+
+
+private:
+ // Custom LU factorization
+ template <typename T, int rowAndCol> //only square matrices
+ void static Crout(const Eigen::Matrix<T, rowAndCol, rowAndCol>& S, Eigen::Matrix<T, rowAndCol, rowAndCol>& D)
+ {
+ for (int k = 0; k < S.rows(); ++k)
+ {
+ for (int i = k; i < S.rows(); ++i)
+ {
+ T sum(0.);
+ for (int p = 0; p < k; ++p)
+ sum += D(i, p) * D(p, k);
+ D(i, k) = S(i, k) - sum; // not dividing by diagonals
+ }
+ for (int j = k + 1; j < S.rows(); ++j)
+ {
+ T sum(0.);
+ for (int p = 0; p < k; ++p)
+ sum += D(k, p) * D(p, j);
+ D(k, j) = (S(k, j) - sum) / D(k, k);
+ }
+ }
+ }
+
+ // Custom LU solver
+ template <typename T, int rowAndCol>
+ void static solveCrout(const Eigen::Matrix<T, rowAndCol, rowAndCol>& LU,
+ const Eigen::Matrix<T, rowAndCol, 1>& b,
+ Eigen::Matrix<T, rowAndCol, 1>& x)
+ {
+ const int d = rowAndCol;
+ T y[d];
+ for (int i = 0; i < d; ++i)
+ {
+ T sum(0.0);
+ for (int k = 0; k < i; ++k)
+ sum += LU(i, k) * y[k];
+ y[i] = (b(i) - sum) / LU(i, i);
+ }
+ for (int i = d - 1; i >= 0; --i)
+ {
+ T sum(0.);
+ for (int k = i + 1; k < d; ++k)
+ sum += LU(i, k) * x(k);
+ x(i) = (y[i] - sum); // not dividing by diagonals
+ }
+ }
+};
+}
+}
+}
diff --git a/ct_rbd/include/ct/rbd/internal/CppADDoubleTrait.h b/ct_rbd/include/ct/rbd/internal/CppADDoubleTrait.h
new file mode 100644
index 0000000..2f6766c
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/internal/CppADDoubleTrait.h
@@ -0,0 +1,40 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <cppad/cppad.hpp>
+#include <cppad/example/cppad_eigen.hpp>
+
+namespace ct {
+namespace rbd {
+namespace internal {
+
+/*!
+ * \brief Trait providing basic mathematical operations for CppAD datatypes
+ */
+struct CppADDoubleTrait
+{
+ typedef CppAD::AD<double> Scalar;
+
+ inline static Scalar sin(const Scalar& x) { return CppAD::sin(x); }
+ inline static Scalar cos(const Scalar& x) { return CppAD::cos(x); }
+ inline static Scalar tan(const Scalar& x) { return CppAD::tan(x); }
+ inline static Scalar sinh(const Scalar& x) { return CppAD::sinh(x); }
+ inline static Scalar cosh(const Scalar& x) { return CppAD::cosh(x); }
+ inline static Scalar tanh(const Scalar& x) { return CppAD::tanh(x); }
+ inline static Scalar exp(const Scalar& x) { return CppAD::exp(x); }
+ inline static Scalar fabs(const Scalar& x) { return CppAD::fabs(x); }
+ template <int Dims>
+ inline static Eigen::Matrix<Scalar, Dims, 1> solve(const Eigen::Matrix<Scalar, Dims, Dims>& A,
+ const Eigen::Matrix<Scalar, Dims, 1>& b)
+ {
+ return A.inverse() * b;
+ }
+};
+}
+}
+}
diff --git a/ct_rbd/include/ct/rbd/internal/TraitSelectorSpecs.h b/ct_rbd/include/ct/rbd/internal/TraitSelectorSpecs.h
new file mode 100644
index 0000000..df74abe
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/internal/TraitSelectorSpecs.h
@@ -0,0 +1,32 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "CppADCodegenTrait.h"
+#include "CppADDoubleTrait.h"
+#include <iit/rbd/traits/TraitSelector.h>
+
+namespace iit {
+namespace rbd {
+namespace tpl {
+
+template <>
+struct TraitSelector<CppAD::AD<double>>
+{
+ typedef ct::rbd::internal::CppADDoubleTrait Trait;
+};
+
+template <>
+struct TraitSelector<CppAD::AD<CppAD::cg::CG<double>>>
+{
+ typedef ct::rbd::internal::CppADCodegenTrait Trait;
+};
+
+
+} //namespace tpl
+} // namespace rbd
+} // namespace iit
diff --git a/ct_rbd/include/ct/rbd/nloc/FixBaseNLOC-impl.h b/ct_rbd/include/ct/rbd/nloc/FixBaseNLOC-impl.h
new file mode 100644
index 0000000..842e650
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/nloc/FixBaseNLOC-impl.h
@@ -0,0 +1,218 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace rbd {
+
+template <class FIX_BASE_FD_SYSTEM, size_t ACTUATOR_STATE_DIM, typename SCALAR>
+FixBaseNLOC<FIX_BASE_FD_SYSTEM, ACTUATOR_STATE_DIM, SCALAR>::FixBaseNLOC(
+ std::shared_ptr<ct::optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>> costFun,
+ const typename NLOptConSolver::Settings_t& nlocSettings,
+ std::shared_ptr<FBSystem> system,
+ bool verbose,
+ std::shared_ptr<LinearizedSystem> linearizedSystem)
+ : system_(system),
+ linearizedSystem_(linearizedSystem),
+ costFunction_(costFun),
+ optConProblem_(system_, costFunction_, linearizedSystem_),
+ iteration_(0)
+{
+ optConProblem_.verify();
+ nlocSolver_ = std::shared_ptr<NLOptConSolver>(new NLOptConSolver(optConProblem_, nlocSettings));
+}
+
+template <class FIX_BASE_FD_SYSTEM, size_t ACTUATOR_STATE_DIM, typename SCALAR>
+void FixBaseNLOC<FIX_BASE_FD_SYSTEM, ACTUATOR_STATE_DIM, SCALAR>::initialize(const tpl::JointState<NJOINTS, SCALAR>& x0,
+ const core::Time& tf,
+ StateVectorArray x_ref,
+ FeedbackArray u0_fb,
+ ControlVectorArray u0_ff)
+{
+ typename NLOptConSolver::Policy_t policy(x_ref, u0_ff, u0_fb, getSettings().dt);
+
+ nlocSolver_->changeTimeHorizon(tf);
+ nlocSolver_->setInitialGuess(policy);
+ nlocSolver_->changeInitialState(x0.toImplementation());
+}
+
+template <class FIX_BASE_FD_SYSTEM, size_t ACTUATOR_STATE_DIM, typename SCALAR>
+void FixBaseNLOC<FIX_BASE_FD_SYSTEM, ACTUATOR_STATE_DIM, SCALAR>::initializeSteadyPose(
+ const tpl::JointState<NJOINTS, SCALAR>& x0,
+ const core::Time& tf,
+ const int N,
+ ControlVector& u_ref,
+ FeedbackMatrix K)
+{
+ ct::core::StateVector<ACTUATOR_STATE_DIM, SCALAR> actRefState;
+ actRefState.setZero();
+
+ ControlVector uff;
+ computeIDTorques(x0, uff); // compute inverse dynamics for this joint state
+ u_ref = uff;
+
+ // transcribe uff into the feed-forward init guess
+ ControlVectorArray u0_ff(N, uff);
+
+ // ... and to the act state if required // todo only works for this special case
+ if (ACTUATOR_STATE_DIM > 0)
+ {
+ actRefState = system_->getActuatorDynamics()->computeStateFromOutput(x0, uff);
+ }
+
+ StateVector x0full = system_->toFullState(x0.toImplementation(), actRefState);
+
+ StateVectorArray x_ref = StateVectorArray(N + 1, x0full);
+ FeedbackArray u0_fb(N, K);
+
+ typename NLOptConSolver::Policy_t policy(x_ref, u0_ff, u0_fb, getSettings().dt);
+
+ nlocSolver_->changeTimeHorizon(tf);
+ nlocSolver_->setInitialGuess(policy);
+ nlocSolver_->changeInitialState(x0full);
+}
+
+template <class FIX_BASE_FD_SYSTEM, size_t ACTUATOR_STATE_DIM, typename SCALAR>
+void FixBaseNLOC<FIX_BASE_FD_SYSTEM, ACTUATOR_STATE_DIM, SCALAR>::initializeDirectInterpolation(
+ const tpl::JointState<NJOINTS, SCALAR>& x0,
+ const tpl::JointState<NJOINTS, SCALAR>& xf,
+ const core::Time& tf,
+ const int N,
+ FeedbackMatrix K)
+{
+ ct::core::ControlVectorArray<NJOINTS, SCALAR> uff_array;
+ ct::core::StateVectorArray<STATE_DIM, SCALAR> x_array;
+
+ initializeDirectInterpolation(x0, xf, tf, N, uff_array, x_array, K);
+}
+
+
+template <class FIX_BASE_FD_SYSTEM, size_t ACTUATOR_STATE_DIM, typename SCALAR>
+void FixBaseNLOC<FIX_BASE_FD_SYSTEM, ACTUATOR_STATE_DIM, SCALAR>::initializeDirectInterpolation(
+ const tpl::JointState<NJOINTS, SCALAR>& x0,
+ const tpl::JointState<NJOINTS, SCALAR>& xf,
+ const core::Time& tf,
+ const int N,
+ ct::core::ControlVectorArray<NJOINTS, SCALAR>& uff_array,
+ ct::core::StateVectorArray<STATE_DIM, SCALAR>& x_array,
+ FeedbackMatrix K)
+{
+ uff_array.resize(N);
+ x_array.resize(N + 1);
+
+ ControlVector uff;
+
+ StateVector x0full = system_->toFullState(x0.toImplementation());
+ StateVector xffull = system_->toFullState(xf.toImplementation());
+
+ for (int i = 0; i < N + 1; i++)
+ {
+ x_array[i] = x0full + (xffull - x0full) * SCALAR(i) / SCALAR(N);
+
+ if (i < N)
+ {
+ ct::rbd::tpl::RBDState<NJOINTS, SCALAR> rbdState = system_->RBDStateFromVector(x_array[i]);
+ const tpl::JointState<NJOINTS, SCALAR> jointState = rbdState.joints();
+ computeIDTorques(jointState, uff_array[i]);
+ }
+ }
+
+ FeedbackArray u0_fb(N, K);
+
+ typename NLOptConSolver::Policy_t policy(x_array, uff_array, u0_fb, getSettings().dt);
+
+ nlocSolver_->changeInitialState(x0full);
+ nlocSolver_->changeTimeHorizon(tf);
+ nlocSolver_->setInitialGuess(policy);
+}
+
+template <class FIX_BASE_FD_SYSTEM, size_t ACTUATOR_STATE_DIM, typename SCALAR>
+bool FixBaseNLOC<FIX_BASE_FD_SYSTEM, ACTUATOR_STATE_DIM, SCALAR>::runIteration()
+{
+ bool foundBetter = nlocSolver_->runIteration();
+
+ iteration_++;
+ return foundBetter;
+}
+
+
+template <class FIX_BASE_FD_SYSTEM, size_t ACTUATOR_STATE_DIM, typename SCALAR>
+bool FixBaseNLOC<FIX_BASE_FD_SYSTEM, ACTUATOR_STATE_DIM, SCALAR>::solve()
+{
+ return nlocSolver_->solve();
+}
+
+template <class FIX_BASE_FD_SYSTEM, size_t ACTUATOR_STATE_DIM, typename SCALAR>
+const core::StateFeedbackController<FixBaseNLOC<FIX_BASE_FD_SYSTEM, ACTUATOR_STATE_DIM, SCALAR>::STATE_DIM,
+ FixBaseNLOC<FIX_BASE_FD_SYSTEM, ACTUATOR_STATE_DIM, SCALAR>::CONTROL_DIM,
+ SCALAR>&
+FixBaseNLOC<FIX_BASE_FD_SYSTEM, ACTUATOR_STATE_DIM, SCALAR>::getSolution()
+{
+ return nlocSolver_->getSolution();
+}
+
+template <class FIX_BASE_FD_SYSTEM, size_t ACTUATOR_STATE_DIM, typename SCALAR>
+const typename FixBaseNLOC<FIX_BASE_FD_SYSTEM, ACTUATOR_STATE_DIM, SCALAR>::StateVectorArray&
+FixBaseNLOC<FIX_BASE_FD_SYSTEM, ACTUATOR_STATE_DIM, SCALAR>::retrieveLastRollout()
+{
+ return nlocSolver_->getStates();
+}
+
+template <class FIX_BASE_FD_SYSTEM, size_t ACTUATOR_STATE_DIM, typename SCALAR>
+const core::TimeArray& FixBaseNLOC<FIX_BASE_FD_SYSTEM, ACTUATOR_STATE_DIM, SCALAR>::getTimeArray()
+{
+ return nlocSolver_->getStateTrajectory().getTimeArray();
+}
+
+template <class FIX_BASE_FD_SYSTEM, size_t ACTUATOR_STATE_DIM, typename SCALAR>
+const typename FixBaseNLOC<FIX_BASE_FD_SYSTEM, ACTUATOR_STATE_DIM, SCALAR>::FeedbackArray&
+FixBaseNLOC<FIX_BASE_FD_SYSTEM, ACTUATOR_STATE_DIM, SCALAR>::getFeedbackArray()
+{
+ return nlocSolver_->getSolution().K();
+}
+
+template <class FIX_BASE_FD_SYSTEM, size_t ACTUATOR_STATE_DIM, typename SCALAR>
+const typename FixBaseNLOC<FIX_BASE_FD_SYSTEM, ACTUATOR_STATE_DIM, SCALAR>::ControlVectorArray&
+FixBaseNLOC<FIX_BASE_FD_SYSTEM, ACTUATOR_STATE_DIM, SCALAR>::getControlVectorArray()
+{
+ return nlocSolver_->getSolution().uff();
+}
+
+template <class FIX_BASE_FD_SYSTEM, size_t ACTUATOR_STATE_DIM, typename SCALAR>
+const typename FixBaseNLOC<FIX_BASE_FD_SYSTEM, ACTUATOR_STATE_DIM, SCALAR>::NLOptConSolver::Settings_t&
+FixBaseNLOC<FIX_BASE_FD_SYSTEM, ACTUATOR_STATE_DIM, SCALAR>::getSettings() const
+{
+ return nlocSolver_->getSettings();
+}
+
+template <class FIX_BASE_FD_SYSTEM, size_t ACTUATOR_STATE_DIM, typename SCALAR>
+void FixBaseNLOC<FIX_BASE_FD_SYSTEM, ACTUATOR_STATE_DIM, SCALAR>::changeCostFunction(
+ std::shared_ptr<CostFunction> costFunction)
+{
+ nlocSolver_->changeCostFunction(costFunction);
+}
+
+template <class FIX_BASE_FD_SYSTEM, size_t ACTUATOR_STATE_DIM, typename SCALAR>
+std::shared_ptr<typename FixBaseNLOC<FIX_BASE_FD_SYSTEM, ACTUATOR_STATE_DIM, SCALAR>::NLOptConSolver>
+FixBaseNLOC<FIX_BASE_FD_SYSTEM, ACTUATOR_STATE_DIM, SCALAR>::getSolver()
+{
+ return nlocSolver_;
+}
+
+template <class FIX_BASE_FD_SYSTEM, size_t ACTUATOR_STATE_DIM, typename SCALAR>
+void FixBaseNLOC<FIX_BASE_FD_SYSTEM, ACTUATOR_STATE_DIM, SCALAR>::computeIDTorques(
+ const tpl::JointState<NJOINTS, SCALAR>& x,
+ ControlVector& u)
+{
+ //! zero external link forces and acceleration
+ JointAcceleration_t jAcc(Eigen::Matrix<SCALAR, NJOINTS, 1>::Zero());
+
+ system_->dynamics().FixBaseID(x, jAcc, u);
+}
+
+} // namespace rbd
+} // namespace ct
diff --git a/ct_rbd/include/ct/rbd/nloc/FixBaseNLOC.h b/ct_rbd/include/ct/rbd/nloc/FixBaseNLOC.h
new file mode 100644
index 0000000..05417fe
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/nloc/FixBaseNLOC.h
@@ -0,0 +1,124 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/rbd/systems/FixBaseFDSystemSymplectic.h>
+
+namespace ct {
+namespace rbd {
+
+/**
+ * \brief NLOC for fixed base systems without an explicit contact model.
+ */
+template <class FIX_BASE_FD_SYSTEM, size_t ACTUATOR_STATE_DIM = 0, typename SCALAR = double>
+class FixBaseNLOC
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ static const bool eeForcesAreControlInputs = false;
+
+ using FBSystem = FIX_BASE_FD_SYSTEM;
+
+ static const size_t CONTROL_DIM = FBSystem::CONTROL_DIM;
+ static const size_t NJOINTS = FBSystem::NJOINTS;
+ static const size_t STATE_DIM = FBSystem::STATE_DIM;
+
+ typedef ct::core::LinearSystem<STATE_DIM, CONTROL_DIM, SCALAR> LinearizedSystem;
+ typedef ct::rbd::RbdLinearizer<FBSystem> SystemLinearizer;
+
+ using JointAcceleration_t = ct::rbd::tpl::JointAcceleration<NJOINTS, SCALAR>;
+
+ //! @ todo: introduce templates for P_DIM and V_DIM
+ typedef ct::optcon::NLOptConSolver<STATE_DIM, CONTROL_DIM, STATE_DIM / 2, STATE_DIM / 2, SCALAR> NLOptConSolver;
+
+ typedef typename core::StateVector<STATE_DIM, SCALAR> StateVector;
+ typedef typename core::ControlVector<CONTROL_DIM, SCALAR> ControlVector;
+ typedef typename core::FeedbackMatrix<STATE_DIM, CONTROL_DIM, SCALAR> FeedbackMatrix;
+ typedef typename core::StateVectorArray<STATE_DIM, SCALAR> StateVectorArray;
+ typedef typename core::ControlVectorArray<CONTROL_DIM, SCALAR> ControlVectorArray;
+ typedef typename core::FeedbackArray<STATE_DIM, CONTROL_DIM, SCALAR> FeedbackArray;
+ typedef typename core::StateFeedbackController<STATE_DIM, CONTROL_DIM, SCALAR> StateFeedbackController;
+
+ typedef ct::optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR> CostFunction;
+
+ //! default constructor
+ FixBaseNLOC() = default;
+
+ //! constructor which directly takes a cost function
+ FixBaseNLOC(std::shared_ptr<ct::optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>> costFun,
+ const typename NLOptConSolver::Settings_t& nlocSettings,
+ std::shared_ptr<FBSystem> system = std::shared_ptr<FBSystem>(new FBSystem),
+ bool verbose = false,
+ std::shared_ptr<LinearizedSystem> linearizedSystem = nullptr);
+
+ void initialize(const tpl::JointState<NJOINTS, SCALAR>& x0,
+ const core::Time& tf,
+ StateVectorArray x_ref = StateVectorArray(),
+ FeedbackArray u0_fb = FeedbackArray(),
+ ControlVectorArray u0_ff = ControlVectorArray());
+
+ //! initialize fixed-base robot with a steady pose using inverse dynamics torques as feedforward
+ void initializeSteadyPose(const tpl::JointState<NJOINTS, SCALAR>& x0,
+ const core::Time& tf,
+ const int N,
+ ControlVector& u_ref,
+ FeedbackMatrix K = FeedbackMatrix::Zero());
+
+ //! initialize fixed-base robot with a directly interpolated state trajectory and corresponding ID torques
+ void initializeDirectInterpolation(const tpl::JointState<NJOINTS, SCALAR>& x0,
+ const tpl::JointState<NJOINTS, SCALAR>& xf,
+ const core::Time& tf,
+ const int N,
+ FeedbackMatrix K = FeedbackMatrix::Zero());
+
+ //! initialize fixed-base robot with a directly interpolated state trajectory and corresponding ID torques
+ void initializeDirectInterpolation(const tpl::JointState<NJOINTS, SCALAR>& x0,
+ const tpl::JointState<NJOINTS, SCALAR>& xf,
+ const core::Time& tf,
+ const int N,
+ ct::core::ControlVectorArray<NJOINTS, SCALAR>& u_array,
+ ct::core::StateVectorArray<STATE_DIM, SCALAR>& x_array,
+ FeedbackMatrix K = FeedbackMatrix::Zero());
+
+ bool runIteration();
+
+ bool solve();
+
+ const core::StateFeedbackController<STATE_DIM, CONTROL_DIM, SCALAR>& getSolution();
+
+ const StateVectorArray& retrieveLastRollout();
+
+ const core::TimeArray& getTimeArray();
+
+ const FeedbackArray& getFeedbackArray();
+
+ const ControlVectorArray& getControlVectorArray();
+
+ const typename NLOptConSolver::Settings_t& getSettings() const;
+
+ void changeCostFunction(std::shared_ptr<CostFunction> costFunction);
+
+ std::shared_ptr<NLOptConSolver> getSolver();
+
+ //! compute fix-base inverse dynamics torques
+ void computeIDTorques(const tpl::JointState<NJOINTS, SCALAR>& x, ControlVector& u);
+
+private:
+ std::shared_ptr<FBSystem> system_;
+ std::shared_ptr<LinearizedSystem> linearizedSystem_;
+ std::shared_ptr<CostFunction> costFunction_;
+
+ optcon::OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR> optConProblem_;
+
+ std::shared_ptr<NLOptConSolver> nlocSolver_;
+
+ size_t iteration_;
+};
+
+} // namespace rbd
+} // namespace ct
diff --git a/ct_rbd/include/ct/rbd/nloc/FloatingBaseNLOCContactModel-impl.h b/ct_rbd/include/ct/rbd/nloc/FloatingBaseNLOCContactModel-impl.h
new file mode 100644
index 0000000..f1a1f45
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/nloc/FloatingBaseNLOCContactModel-impl.h
@@ -0,0 +1,123 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace rbd {
+
+template <class RBDDynamics>
+FloatingBaseNLOCContactModel<RBDDynamics>::FloatingBaseNLOCContactModel(const std::string& costFunctionFile,
+ const std::string& settingsFile,
+ std::shared_ptr<FBSystem> system,
+ std::shared_ptr<LinearizedSystem> linearizedSystem)
+ : system_(system),
+ linearizedSystem_(linearizedSystem),
+ costFunction_(new CostFunction(costFunctionFile, false)),
+ optConProblem_(system_, costFunction_, linearizedSystem_),
+ iteration_(0)
+{
+ solver_ = std::shared_ptr<NLOptConSolver>(new NLOptConSolver(optConProblem_, settingsFile));
+}
+
+template <class RBDDynamics>
+FloatingBaseNLOCContactModel<RBDDynamics>::FloatingBaseNLOCContactModel(const std::string& costFunctionFile,
+ const typename NLOptConSolver::Settings_t& settings,
+ std::shared_ptr<FBSystem> system,
+ std::shared_ptr<LinearizedSystem> linearizedSystem)
+ : system_(system),
+ linearizedSystem_(linearizedSystem),
+ costFunction_(new CostFunction(costFunctionFile, false)),
+ optConProblem_(system_, costFunction_, linearizedSystem_),
+ iteration_(0)
+{
+ solver_ = std::shared_ptr<NLOptConSolver>(new NLOptConSolver(optConProblem_, settings));
+}
+
+template <class RBDDynamics>
+void FloatingBaseNLOCContactModel<RBDDynamics>::initialize(const typename RBDDynamics::RBDState_t& x0,
+ const core::Time& tf,
+ StateVectorArray x_ref,
+ FeedbackArray u0_fb,
+ ControlVectorArray u0_ff)
+{
+ typename NLOptConSolver::Policy_t policy(x_ref, u0_ff, u0_fb, getSettings().dt);
+
+ solver_->changeTimeHorizon(tf);
+ solver_->setInitialGuess(policy);
+ solver_->changeInitialState(x0.toStateVectorEulerXyz());
+}
+
+template <class RBDDynamics>
+void FloatingBaseNLOCContactModel<RBDDynamics>::configure(const typename NLOptConSolver::Settings_t& settings)
+{
+ solver_->configure(settings);
+}
+
+template <class RBDDynamics>
+bool FloatingBaseNLOCContactModel<RBDDynamics>::runIteration()
+{
+ bool foundBetter = solver_->runIteration();
+
+ iteration_++;
+ return foundBetter;
+}
+
+template <class RBDDynamics>
+const typename FloatingBaseNLOCContactModel<RBDDynamics>::StateVectorArray&
+FloatingBaseNLOCContactModel<RBDDynamics>::retrieveLastRollout()
+{
+ return solver_->getStates();
+}
+
+template <class RBDDynamics>
+const typename FloatingBaseNLOCContactModel<RBDDynamics>::StateVectorArray&
+FloatingBaseNLOCContactModel<RBDDynamics>::getStateVectorArray()
+{
+ return solver_->getSolution().x_ref();
+}
+
+template <class RBDDynamics>
+const core::TimeArray& FloatingBaseNLOCContactModel<RBDDynamics>::getTimeArray()
+{
+ return solver_->getStateTrajectory().getTimeArray();
+}
+
+template <class RBDDynamics>
+const typename FloatingBaseNLOCContactModel<RBDDynamics>::FeedbackArray&
+FloatingBaseNLOCContactModel<RBDDynamics>::getFeedbackArray()
+{
+ return solver_->getSolution().K();
+}
+
+template <class RBDDynamics>
+const typename FloatingBaseNLOCContactModel<RBDDynamics>::ControlVectorArray&
+FloatingBaseNLOCContactModel<RBDDynamics>::getControlVectorArray()
+{
+ return solver_->getSolution().uff();
+}
+
+template <class RBDDynamics>
+const typename FloatingBaseNLOCContactModel<RBDDynamics>::NLOptConSolver::Settings_t&
+FloatingBaseNLOCContactModel<RBDDynamics>::getSettings() const
+{
+ return solver_->getSettings();
+}
+
+template <class RBDDynamics>
+void FloatingBaseNLOCContactModel<RBDDynamics>::changeCostFunction(std::shared_ptr<CostFunction> costFunction)
+{
+ solver_->changeCostFunction(costFunction);
+}
+
+template <class RBDDynamics>
+std::shared_ptr<typename FloatingBaseNLOCContactModel<RBDDynamics>::NLOptConSolver>
+FloatingBaseNLOCContactModel<RBDDynamics>::getSolver()
+{
+ return solver_;
+}
+}
+}
diff --git a/ct_rbd/include/ct/rbd/nloc/FloatingBaseNLOCContactModel.h b/ct_rbd/include/ct/rbd/nloc/FloatingBaseNLOCContactModel.h
new file mode 100644
index 0000000..fda1af4
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/nloc/FloatingBaseNLOCContactModel.h
@@ -0,0 +1,94 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/rbd/systems/FloatingBaseFDSystem.h>
+#include <ct/rbd/systems/linear/RbdLinearizer.h>
+
+namespace ct {
+namespace rbd {
+
+/**
+ * \brief NLOC for floating base systems with an explicit contact model.
+ */
+template <class RBDDynamics>
+class FloatingBaseNLOCContactModel
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ static const bool quatIntegration = false;
+ static const bool eeForcesAreControlInputs = false;
+
+ typedef FloatingBaseFDSystem<RBDDynamics, false, false> FBSystem;
+ typedef ct::core::LinearSystem<FBSystem::STATE_DIM, FBSystem::CONTROL_DIM> LinearizedSystem;
+ typedef ct::rbd::RbdLinearizer<FBSystem> RBDLinearizer;
+ typedef ct::core::SystemLinearizer<FBSystem::STATE_DIM, FBSystem::CONTROL_DIM> SystemLinearizer;
+
+ typedef ct::optcon::NLOptConSolver<FBSystem::STATE_DIM, FBSystem::CONTROL_DIM> NLOptConSolver;
+
+ typedef typename core::StateVector<FBSystem::STATE_DIM> StateVector;
+ typedef typename core::ControlVector<FBSystem::CONTROL_DIM> ControlVector;
+ typedef typename core::FeedbackMatrix<FBSystem::STATE_DIM, FBSystem::CONTROL_DIM> FeedbackMatrix;
+ typedef typename core::StateVectorArray<FBSystem::STATE_DIM> StateVectorArray;
+ typedef typename core::ControlVectorArray<FBSystem::CONTROL_DIM> ControlVectorArray;
+ typedef typename core::FeedbackArray<FBSystem::STATE_DIM, FBSystem::CONTROL_DIM> FeedbackArray;
+
+ typedef ct::optcon::CostFunctionAnalytical<FBSystem::STATE_DIM, FBSystem::CONTROL_DIM> CostFunction;
+
+
+ //! constructor taking path to the settings file
+ FloatingBaseNLOCContactModel(const std::string& costFunctionFile,
+ const std::string& settingsFile,
+ std::shared_ptr<FBSystem> system = std::shared_ptr<FBSystem>(new FBSystem),
+ std::shared_ptr<LinearizedSystem> linearizedSystem = nullptr);
+
+ //! constructor taking settings file directly
+ FloatingBaseNLOCContactModel(const std::string& costFunctionFile,
+ const typename NLOptConSolver::Settings_t& settings,
+ std::shared_ptr<FBSystem> system = std::shared_ptr<FBSystem>(new FBSystem),
+ std::shared_ptr<LinearizedSystem> linearizedSystem = nullptr);
+
+ void initialize(const typename RBDDynamics::RBDState_t& x0,
+ const core::Time& tf,
+ StateVectorArray x_ref = StateVectorArray(),
+ FeedbackArray u0_fb = FeedbackArray(),
+ ControlVectorArray u0_ff = ControlVectorArray());
+
+ void configure(const typename NLOptConSolver::Settings_t& settings);
+
+ bool runIteration();
+
+ const StateVectorArray& retrieveLastRollout();
+
+ const StateVectorArray& getStateVectorArray();
+
+ const core::TimeArray& getTimeArray();
+
+ const FeedbackArray& getFeedbackArray();
+
+ const ControlVectorArray& getControlVectorArray();
+
+ const typename NLOptConSolver::Settings_t& getSettings() const;
+
+ void changeCostFunction(std::shared_ptr<CostFunction> costFunction);
+
+ std::shared_ptr<NLOptConSolver> getSolver();
+
+private:
+ std::shared_ptr<FBSystem> system_;
+ std::shared_ptr<LinearizedSystem> linearizedSystem_;
+ std::shared_ptr<CostFunction> costFunction_;
+
+ optcon::OptConProblem<FBSystem::STATE_DIM, FBSystem::CONTROL_DIM> optConProblem_;
+
+ std::shared_ptr<NLOptConSolver> solver_;
+
+ size_t iteration_;
+};
+}
+}
diff --git a/ct_rbd/include/ct/rbd/operationalSpace/coordinate/CoordinateBase.h b/ct_rbd/include/ct/rbd/operationalSpace/coordinate/CoordinateBase.h
new file mode 100644
index 0000000..f0e4332
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/operationalSpace/coordinate/CoordinateBase.h
@@ -0,0 +1,34 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/rbd/state/RBDState.h>
+
+namespace ct {
+namespace rbd {
+
+/**
+ * \ingroup OS
+ * @Brief This class is an interface class for operational space coordinate.
+ */
+template <size_t NUM_OUTPUTS, size_t NUM_JOINTS>
+class CoordinateBase
+{
+public:
+ typedef std::shared_ptr<CoordinateBase<NUM_OUTPUTS, NUM_JOINTS>> ptr;
+ typedef RBDState<NUM_JOINTS> state_t;
+ typedef Eigen::Matrix<double, NUM_OUTPUTS, 1> coordinate_t;
+
+ CoordinateBase() {}
+ virtual ~CoordinateBase() {}
+ virtual coordinate_t getCoordinate(const state_t& state) = 0;
+
+private:
+};
+
+} // namespace rbd
+} // namespace ct
diff --git a/ct_rbd/include/ct/rbd/operationalSpace/rigid_body/OperationalModel.h b/ct_rbd/include/ct/rbd/operationalSpace/rigid_body/OperationalModel.h
new file mode 100644
index 0000000..5241542
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/operationalSpace/rigid_body/OperationalModel.h
@@ -0,0 +1,130 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "OperationalModelBase.h"
+
+namespace ct {
+namespace rbd {
+
+/**
+ * \ingroup OS
+ * \brief This is the class for the operational space model which gives access to the operational model parameter.
+ * The model is assumed to have the following form:
+ * \f$ M \ddot{x} + C + G = S^\top \tau + J_c^\top \lambda \f$
+ * where \f$ \ddot{x} \f$ is the acceleration of the operational space coordinate.
+ */
+template <size_t NUM_OUTPUTS, size_t NUM_JOINTS, size_t NUM_CONTACTPOINTS>
+class OperationalModel : public OperationalModelBase<NUM_OUTPUTS, NUM_JOINTS, NUM_CONTACTPOINTS>
+{
+public:
+ typedef std::shared_ptr<OperationalModel<NUM_OUTPUTS, NUM_JOINTS, NUM_CONTACTPOINTS>> ptr;
+ typedef typename OperationalModelBase<NUM_JOINTS + 6, NUM_JOINTS, NUM_CONTACTPOINTS>::ptr
+ full_body_model_ptr_t; // this is a pointer type to original system
+ typedef OperationalModelBase<NUM_OUTPUTS, NUM_JOINTS, NUM_CONTACTPOINTS> Base;
+ typedef typename Base::state_t state_t;
+ typedef typename Base::jacobian_t jacobian_t; //JacobianType;
+ typedef typename Base::jacobian_inv_t jacobian_inv_t; //JacobianInverseType;
+ typedef typename Base::jacobian_class_ptr_t jacobian_class_ptr_t; //JacobianClassType;
+ typedef typename Base::coordinate_class_ptr_t coordinate_class_ptr_t; //CoordinateClassType;
+
+ OperationalModel(const full_body_model_ptr_t& fullModelPtr,
+ const jacobian_class_ptr_t& jacobianPtr,
+ const coordinate_class_ptr_t& coordinatePtr = nullptr)
+ : Base(jacobianPtr, coordinatePtr), fullModelPtr_(fullModelPtr), jacobianPtr_(jacobianPtr)
+ {
+ if (!fullModelPtr)
+ throw std::runtime_error("Model pointer in is not instantiated!");
+ if (!jacobianPtr)
+ throw std::runtime_error("Task space Jacobian pointer in is not instantiated!");
+ }
+
+ OperationalModel() = delete;
+ ~OperationalModel(){};
+
+ /**
+ * This method updates the class member variables using the current state.
+ * @param state The current state
+ */
+ void update(const state_t& state) override
+ {
+ Base::state_ = state;
+
+ fullModelPtr_->update(state);
+ Base::jacobianPtr_->updateState(state, fullModelPtr_->MInverse());
+
+ // jacobian dager
+ const jacobian_inv_t& Jdager = Base::jacobianPtr_->JdagerLDLT();
+
+ // rigid body coefficients
+ Base::M_ = Jdager.transpose() * fullModelPtr_->M() * Jdager;
+ Base::MInverse_ = Base::M_.ldlt().solve(Eigen::MatrixXd::Identity(NUM_OUTPUTS, NUM_OUTPUTS));
+ Base::C_ = Jdager.transpose() * fullModelPtr_->C() -
+ Base::M_ * Base::jacobianPtr_->dJdt() * state.toCoordinateVelocity();
+ Base::G_ = Jdager.transpose() * fullModelPtr_->G();
+ Base::S_ = fullModelPtr_->S() * Jdager;
+ // contact jacobians
+ for (size_t i = 0; i < NUM_CONTACTPOINTS; i++)
+ Base::AllJc_[i] = fullModelPtr_->Jc(i) * Jdager;
+ }
+
+ /**
+ * this method returns a pointer to the internal operational space Jacobian.
+ * @return pointer to the internal operational space Jacobian
+ */
+ jacobian_class_ptr_t getJacobianPtr() { return jacobianPtr_; }
+ const typename Base::S_t& STransposeDager(
+ const Eigen::MatrixXd& orthogonalSystemS = Eigen::MatrixXd::Zero(NUM_JOINTS, 0))
+ {
+ Eigen::Matrix<double, NUM_JOINTS, NUM_JOINTS> fixedBaseMassMatrix =
+ (fullModelPtr_->S() * fullModelPtr_->MInverse() * fullModelPtr_->S().transpose())
+ .ldlt()
+ .solve(Eigen::MatrixXd::Identity(NUM_JOINTS, NUM_JOINTS));
+
+ Eigen::Matrix<double, NUM_JOINTS, NUM_JOINTS> W0 = fixedBaseMassMatrix;
+ W0.setIdentity();
+
+ Eigen::Matrix<double, NUM_JOINTS, NUM_JOINTS> W;
+ if (orthogonalSystemS.cols() != 0)
+ W = W0 -
+ W0 * orthogonalSystemS *
+ (orthogonalSystemS.transpose() * W0 * orthogonalSystemS)
+ .ldlt()
+ .solve(Eigen::MatrixXd::Identity(orthogonalSystemS.cols(), orthogonalSystemS.cols())) *
+ orthogonalSystemS.transpose() * W0;
+ else
+ W = W0;
+
+ STransposeDager_ =
+ W * Base::S_ *
+ (Base::S_.transpose() * W * Base::S_).ldlt().solve(Eigen::MatrixXd::Identity(NUM_OUTPUTS, NUM_OUTPUTS));
+ return STransposeDager_;
+ }
+
+private:
+ full_body_model_ptr_t fullModelPtr_;
+ jacobian_class_ptr_t jacobianPtr_;
+ typename Base::S_t STransposeDager_;
+};
+
+
+template <>
+inline void OperationalModel<0, 12, 4>::update(const state_t& state)
+{
+ Base::state_ = state;
+ fullModelPtr_->update(state);
+}
+
+template <>
+inline const OperationalModel<0, 12, 4>::Base::S_t& OperationalModel<0, 12, 4>::STransposeDager(
+ const Eigen::MatrixXd& orthogonalSystemS)
+{
+ STransposeDager_.setZero();
+ return STransposeDager_;
+}
+}
+}
diff --git a/ct_rbd/include/ct/rbd/operationalSpace/rigid_body/OperationalModelBase.h b/ct_rbd/include/ct/rbd/operationalSpace/rigid_body/OperationalModelBase.h
new file mode 100644
index 0000000..29247c6
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/operationalSpace/rigid_body/OperationalModelBase.h
@@ -0,0 +1,117 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+
+#include <ct/rbd/robot/jacobian/OperationalJacobianBase.h>
+#include <ct/rbd/state/RBDState.h>
+#include "../coordinate/CoordinateBase.h"
+
+namespace ct {
+namespace rbd {
+
+/**
+ * \defgroup OS OperationalSpace
+ * \brief Operational Space module is a group of classes for implementing the operational space model.
+ */
+
+/**
+ * \ingroup OS
+ * \Brief This is the base class for the operational space model which gives access to the model parameter.
+ * The model is assumed to have the following form:
+ * \f$ M \ddot{x} + C + G = S^\top \tau + J_c^\top \lambda \f$
+ * where \f$ \ddot{x} \f$ is the acceleration of the operational space coordinate.
+ */
+template <size_t NUM_OUTPUTS, size_t NUM_JOINTS, size_t NUM_CONTACTPOINTS>
+class OperationalModelBase
+{
+public:
+ typedef std::shared_ptr<OperationalModelBase<NUM_OUTPUTS, NUM_JOINTS, NUM_CONTACTPOINTS>> ptr;
+
+ typedef typename CoordinateBase<NUM_OUTPUTS, NUM_JOINTS>::ptr coordinate_class_ptr_t; //CoordinateClassType;
+ typedef typename OperationalJacobianBase<NUM_OUTPUTS, NUM_JOINTS>::ptr jacobian_class_ptr_t; //JacobianClassType;
+
+ typedef typename OperationalJacobianBase<NUM_OUTPUTS, NUM_JOINTS>::jacobian_t jacobian_t; //JacobianType;
+ typedef typename OperationalJacobianBase<NUM_OUTPUTS, NUM_JOINTS>::jacobian_inv_t
+ jacobian_inv_t; //JacobianInverseType;
+
+ typedef RBDState<NUM_JOINTS> state_t;
+ typedef Eigen::Matrix<double, NUM_OUTPUTS, NUM_OUTPUTS> M_t;
+ typedef Eigen::Matrix<double, NUM_OUTPUTS, 1> C_t;
+ typedef Eigen::Matrix<double, NUM_OUTPUTS, 1> G_t;
+ typedef Eigen::Matrix<double, NUM_JOINTS, NUM_OUTPUTS> S_t;
+ typedef Eigen::Matrix<double, 3, NUM_OUTPUTS> Jc_t;
+
+ OperationalModelBase(const jacobian_class_ptr_t& jacobianPtr = nullptr,
+ const coordinate_class_ptr_t& coordinatePtr = nullptr)
+ : jacobianPtr_(jacobianPtr), coordinatePtr_(coordinatePtr){};
+
+ virtual ~OperationalModelBase(){};
+
+ //! Get the inertia matrix: M
+ const M_t& M() const { return M_; }
+ //! Get the inertia matrix inverse: M^{-1}
+ const M_t& MInverse() const { return MInverse_; }
+ //! Get the Coriolis force vector: C
+ const C_t& C() const { return C_; }
+ //! Get the gravity force vector: G
+ const G_t& G() const { return G_; }
+ //! Get the selection matrix: S
+ const S_t& S() const { return S_; }
+ //! Get all of the contact Jacobians: Jc
+ const std::array<Jc_t, NUM_CONTACTPOINTS>& AllJc() const { return AllJc_; }
+ //! Get the ith contact Jacobian: Jc[i]
+ const Jc_t& Jc(size_t i) const { return AllJc_[i]; }
+ //! Get the coordinate position
+ Eigen::Matrix<double, NUM_OUTPUTS, 1> getPositions()
+ {
+ if (coordinatePtr_)
+ return coordinatePtr_->getCoordinate(state_);
+ else
+ return state_.toCoordinatePosition().template head<NUM_OUTPUTS>();
+ }
+
+ //! Get the coordinate velocity
+ Eigen::Matrix<double, NUM_OUTPUTS, 1> getVelocities()
+ {
+ if (jacobianPtr_)
+ return jacobianPtr_->J() * state_.toCoordinateVelocity();
+ else
+ return state_.toCoordinateVelocity().template head<NUM_OUTPUTS>();
+ }
+
+ //! Get the coordinate acceleration
+ Eigen::Matrix<double, NUM_OUTPUTS, 1> getAccelerations(Eigen::Matrix<double, NUM_JOINTS + 6, 1> qdd)
+ {
+ if (jacobianPtr_)
+ return jacobianPtr_->J() * qdd + jacobianPtr_->dJdt() * state_.toCoordinateVelocity();
+ else
+ return qdd.template head<NUM_OUTPUTS>();
+ }
+
+ /**
+ * This method updates the class member variables using the current state.
+ * @param state The current state
+ */
+ virtual void update(const state_t& state) = 0;
+
+protected:
+ jacobian_class_ptr_t jacobianPtr_;
+ coordinate_class_ptr_t coordinatePtr_;
+
+ state_t state_;
+
+ M_t M_;
+ M_t MInverse_;
+ C_t C_;
+ G_t G_;
+ S_t S_;
+ std::array<Jc_t, NUM_CONTACTPOINTS> AllJc_;
+};
+
+} // end of rbd namespace
+} // end of os namespace
diff --git a/ct_rbd/include/ct/rbd/operationalSpace/rigid_body/OperationalModelRBD.h b/ct_rbd/include/ct/rbd/operationalSpace/rigid_body/OperationalModelRBD.h
new file mode 100644
index 0000000..fafc845
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/operationalSpace/rigid_body/OperationalModelRBD.h
@@ -0,0 +1,105 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/rbd/robot/jacobian/FrameJacobian.h>
+#include <ct/rbd/robot/kinematics/EndEffector.h>
+#include "OperationalModelBase.h"
+
+namespace ct {
+namespace rbd {
+namespace tpl {
+
+/**
+ * \ingroup OS
+ * \brief This is a class for expressing the RBD equations of an articulated robot as an operational model.
+ * It uses the RBDContainer class as an interface to access the generated code for an articulated robot.
+ */
+template <class RBDContainer, size_t NUM_CONTACTPOINTS, typename SCALAR = double>
+class OperationalModelRBD
+ : public OperationalModelBase<RBDContainer::NJOINTS + 6, RBDContainer::NJOINTS, NUM_CONTACTPOINTS>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR, 3, 3> Matrix3s;
+ typedef Eigen::Matrix<SCALAR, 3, 1> Vector3s;
+
+ enum
+ {
+ NUM_OUTPUTS = RBDContainer::NJOINTS + 6,
+ NUM_JOINTS = RBDContainer::NJOINTS,
+ };
+
+ typedef std::shared_ptr<OperationalModelRBD<RBDContainer, NUM_CONTACTPOINTS>> ptr;
+ typedef OperationalModelBase<NUM_OUTPUTS, NUM_JOINTS, NUM_CONTACTPOINTS> Base;
+ typedef typename Base::state_t state_t;
+
+ OperationalModelRBD(const typename RBDContainer::Ptr_t& rbdContainerPtr,
+ const std::array<EndEffector<NUM_JOINTS>, NUM_CONTACTPOINTS>& endEffectorArray)
+ : rbdContainerPtr_(rbdContainerPtr), endEffectorArray_(endEffectorArray)
+ {
+ }
+
+ ~OperationalModelRBD() {}
+ /*!
+ * This method updates the class member variables using the current state.
+ * @param state The current state
+ */
+ void update(const state_t& state) override
+ {
+ Base::state_ = state;
+
+ // rotation matrix
+ Matrix3s o_R_b = state.basePose().getRotationMatrix().toImplementation();
+
+ // Mass matrix
+ Base::M_ = rbdContainerPtr_->jSim().update(state.jointPositions());
+ Base::MInverse_ = Base::M_.ldlt().solve(Eigen::MatrixXd::Identity(18, 18));
+
+ rbdContainerPtr_->inverseDynamics().setJointStatus(state.jointPositions());
+ Eigen::Matrix<SCALAR, 6, 1> baseWrench;
+ Eigen::Matrix<SCALAR, 12, 1> jointForces;
+
+ // centrifugal vector
+ Eigen::Matrix<SCALAR, 6, 1> trunkVel;
+ trunkVel << state.baseLocalAngularVelocity().toImplementation(), state.baseLinearVelocity().toImplementation();
+ rbdContainerPtr_->inverseDynamics().C_terms_fully_actuated(
+ baseWrench, jointForces, trunkVel, state.jointVelocities());
+ Base::C_ << baseWrench, jointForces;
+
+ // gravity vector
+ rbdContainerPtr_->inverseDynamics().G_terms_fully_actuated(
+ baseWrench, jointForces, state.basePose().computeGravityB6D());
+ Base::G_ << baseWrench, jointForces;
+
+ // Selection matrix
+ Base::S_ << Eigen::MatrixXd::Zero(12, 6), Eigen::MatrixXd::Identity(12, 12);
+
+ // contact jacobians
+ for (size_t j = 0; j < NUM_CONTACTPOINTS; j++)
+ {
+ Vector3s b_r_f = rbdContainerPtr_->getEEPositionInBase(j, state.jointPositions()).toImplementation();
+ Eigen::Matrix<SCALAR, 3, NUM_JOINTS> b_J_f =
+ rbdContainerPtr_->getJacobianBaseEEbyId(j, state.jointPositions()).template bottomRows<3>();
+
+ FrameJacobian<NUM_JOINTS, SCALAR>::FromBaseJacToInertiaJacTranslation(o_R_b, b_r_f, b_J_f, Base::AllJc_[j]);
+ } // end of j loop
+ }
+
+private:
+ typename RBDContainer::Ptr_t rbdContainerPtr_;
+ std::array<EndEffector<NUM_JOINTS>, NUM_CONTACTPOINTS> endEffectorArray_;
+};
+
+} // namespace tpl
+
+template <class RBDContainer, size_t NUM_CONTACTPOINTS>
+using OperationalModelRBD = tpl::OperationalModelRBD<RBDContainer, NUM_CONTACTPOINTS, double>;
+
+} // end of rbd namespace
+} // end of os namespace
diff --git a/ct_rbd/include/ct/rbd/physics/EEContactModel.h b/ct_rbd/include/ct/rbd/physics/EEContactModel.h
new file mode 100644
index 0000000..ca27339
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/physics/EEContactModel.h
@@ -0,0 +1,296 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <kindr/Core>
+
+#include <ct/rbd/state/RBDState.h>
+
+#include <iit/rbd/rbd.h>
+#include <iit/rbd/robcogen_commons.h>
+#include <iit/rbd/traits/TraitSelector.h>
+
+namespace ct {
+namespace rbd {
+
+
+/*!
+ * \brief A soft contact model that only uses end-effector positions/velocities to compute the contact force
+ *
+ * This contact model computes forces/torques at the endeffectors given the current state of the robot expressed
+ * in generalized coordinates
+ *
+ * \f[ {}_W \lambda = f(q, \dot{q}) \f]
+ *
+ * The contact model assumes a plane with fixed orientation located at the origin (0, 0, 0). The contact dynamics
+ * are a combination of a spring-damper perpendicular and a damper in parallel to the surface. The force expressed
+ * in world coordinates without velocity smoothing or normal force smoothing is defined as
+ *
+ * \f[ {}_W \lambda = f(q, \dot{q}) = - k ({}_W x_z - z_{offset}) - d {}_W \dot{x} \f]
+ *
+ * where \f$ {}_W x_z \f$ is the ground penetration with respect to an offset \f$ z_{offset} \f$ expressed in world
+ * coordinates and \f$ \dot{x} \f$ is the velocity of the endeffector.
+ *
+ * In case normal force smoothing is activated, the first term becomes
+ *
+ * \f[ {}_W \lambda_n = k e^{-\alpha_n {}_W x_z} \f]
+ *
+ * In case velocity smoothing is activated, the second term becomes
+ *
+ * \f[ {}_W \lambda_t = s({}_W x_z, \dot{x}) ~ d {}_W \dot{x}_{xy} \f]
+ *
+ * where the smoothing coefficient is one of the following
+ *
+ * 1. sigmoid
+ * \f[ s({}_W x_z, \dot{x}) = \frac{1}{1 + e^{{}_W x_z \alpha}} \f]
+ *
+ * 2. tanh (same as sigmoid but computed differently)
+ * \f[ s({}_W x_z, \dot{x}) = \frac{1}{2} tanh(-\frac{1}{2} {}_W x_z \alpha) + \frac{1}{2} \f]
+ *
+ * 3. abs
+ * \f[ s({}_W x_z, \dot{x}) = - \frac{1}{2} \frac{{}_W x_z \alpha}{1 + abs(-{}_W x_z \alpha)} + \frac{1}{2} \f]
+ *
+ * \tparam Kinematics the Kinematics implementation of the robot
+ */
+template <class Kinematics>
+class EEContactModel
+{
+public:
+ static const size_t NUM_EE = Kinematics::NUM_EE;
+ static const size_t NJOINTS = Kinematics::NJOINTS;
+
+ typedef typename Kinematics::SCALAR SCALAR;
+
+ typedef typename iit::rbd::tpl::TraitSelector<SCALAR>::Trait TRAIT;
+
+ typedef std::array<bool, NUM_EE> ActiveMap;
+ typedef typename Kinematics::EEForceLinear EEForceLinear;
+ typedef std::array<EEForceLinear, NUM_EE> EEForcesLinear;
+
+ typedef Eigen::Matrix<SCALAR, 3, 1> Vector3s;
+ typedef kindr::Position<SCALAR, 3> Position3S;
+ typedef kindr::Velocity<SCALAR, 3> Velocity3S;
+
+
+ /*!
+ * \brief the type of velcity smoothing
+ */
+ enum VELOCITY_SMOOTHING
+ {
+ NONE = 0, //!< none
+ SIGMOID = 1, //!< sigmoid
+ TANH = 2, //!< tanh
+ ABS = 3 //!< abs
+ };
+
+ /*!
+ * \brief Default constructor
+ * @param k stiffness of vertical spring
+ * @param d damper coefficient
+ * @param alpha velocity smoothing coefficient
+ * @param alpha_n normal force smoothing coefficient
+ * @param zOffset z offset of the plane with respect to (0, 0, 0)
+ * @param smoothing the type of velocity smoothing
+ * @param kinematics instance of the kinematics (optionally). Should be provided for efficiency when using Auto-Diff.
+ */
+ EEContactModel(const SCALAR& k = SCALAR(5000),
+ const SCALAR& d = SCALAR(500.0),
+ const SCALAR& alpha = SCALAR(100.0),
+ const SCALAR& alpha_n = SCALAR(-1.0),
+ const SCALAR& zOffset = SCALAR(0.0),
+ const VELOCITY_SMOOTHING& smoothing = NONE,
+ const std::shared_ptr<Kinematics>& kinematics = std::shared_ptr<Kinematics>(new Kinematics()))
+ : kinematics_(kinematics),
+ smoothing_(smoothing),
+ k_(k),
+ d_(d),
+ alpha_(alpha),
+ alpha_n_(alpha_n),
+ zOffset_(zOffset)
+ {
+ for (size_t i = 0; i < NUM_EE; i++)
+ EEactive_[i] = true;
+ }
+
+ /*!
+ * \brief Clone operator
+ * @param other instance to clone
+ */
+ EEContactModel(const EEContactModel& other)
+ : kinematics_(other.kinematics_->clone()),
+ smoothing_(other.smoothing_),
+ k_(other.k_),
+ d_(other.d_),
+ alpha_(other.alpha_),
+ alpha_n_(other.alpha_n_),
+ zOffset_(other.zOffset_),
+ EEactive_(other.EEactive_)
+ {
+ }
+
+ EEContactModel* clone() const { return new EEContactModel(*this); }
+ /**
+ * \brief Sets which end-effectors can have forces excerted on them
+ * @param activeMap flags of active end-effectors
+ */
+ void setActiveEE(const ActiveMap& activeMap) { EEactive_ = activeMap; }
+ /**
+ * \brief Computes the contact forces given a state of the robot. Returns forces expressed in the world frame
+ * @param state The state of the robot
+ * @return End-effector forces expressed in the world frame
+ */
+ EEForcesLinear computeContactForces(const tpl::RBDState<NJOINTS, SCALAR>& state)
+ {
+ EEForcesLinear eeForces;
+
+ for (size_t i = 0; i < NUM_EE; i++)
+ {
+ if (EEactive_[i])
+ {
+ Vector3s eePenetration = computePenetration(i, state.basePose(), state.jointPositions());
+
+ if (eeInContact(eePenetration))
+ {
+ Velocity3S eeVelocity = kinematics_->getEEVelocityInWorld(i, state);
+ eeForces[i] = computeEEForce(eePenetration, eeVelocity);
+ }
+ else
+ {
+ eeForces[i].setZero();
+ }
+ }
+ }
+
+ return eeForces;
+ }
+
+ SCALAR& alpha() { return alpha_; }
+ SCALAR& alpha_n() { return alpha_n_; }
+ SCALAR& k() { return k_; }
+ SCALAR& d() { return d_; }
+ SCALAR& zOffset() { return zOffset_; }
+ VELOCITY_SMOOTHING& smoothing() { return smoothing_; }
+private:
+ /**
+ * \brief Checks if end-effector is in contact. Currently assumes this is the case for negative z
+ * @param eePenetration The surface penetration of the end-effector
+ * @return flag if the end-effector is in contact
+ */
+ bool eeInContact(const Vector3s& eePenetration)
+ {
+ if (smoothing_ == NONE && eePenetration(2) > 0.0)
+ return false;
+ else
+ return true;
+ }
+
+
+ /**
+ * \brief Computes the surface penetration. Currently assumes the surface is at height z = 0.
+ * @param eeId ID of the end-effector
+ * @param basePose Position of the robot base
+ * @param jointPosition Joint position of the robot
+ * @return Penetration in world coordinates
+ */
+ Vector3s computePenetration(const size_t& eeId,
+ const tpl::RigidBodyPose<SCALAR>& basePose,
+ const typename tpl::JointState<NJOINTS, SCALAR>::Position& jointPosition)
+ {
+ Position3S pos = kinematics_->getEEPositionInWorld(eeId, basePose, jointPosition);
+
+ // we currently assume flat ground at height zero penetration is only z height
+ Vector3s penetration;
+ penetration << SCALAR(0.0), SCALAR(0.0), pos.z();
+
+ return penetration;
+ }
+
+ /*!
+ * \brief Compute the endeffector force based on penetration and velocity
+ * @param eePenetration end-effector penetration
+ * @param eeVelocity end-effector velocity
+ * @return resulting force vecttor
+ */
+ EEForceLinear computeEEForce(const Vector3s& eePenetration, const Velocity3S& eeVelocity)
+ {
+ EEForceLinear eeForce;
+
+ computeDamperForce(eeForce, eePenetration, eeVelocity);
+
+ smoothEEForce(eeForce, eePenetration);
+
+ computeNormalSpring(eeForce, eePenetration(2) - zOffset_, eeVelocity.toImplementation()(2));
+
+ return eeForce;
+ }
+
+ /*!
+ * \brief Smoothes out the endeffector forces
+ * @param eeForce endeffector force to modify
+ * @param eePenetration penetration of the surface
+ */
+ void smoothEEForce(EEForceLinear& eeForce, const Vector3s& eePenetration)
+ {
+ switch (smoothing_)
+ {
+ case NONE:
+ return;
+ case SIGMOID:
+ eeForce *= 1. / (1. + TRAIT::exp(eePenetration(2) * alpha_));
+ return;
+ case TANH:
+ // same as sigmoid, maybe cheaper / more expensive to compute?
+ eeForce *= 0.5 * TRAIT::tanh(-0.5 * eePenetration(2) * alpha_) + 0.5;
+ return;
+ case ABS:
+ eeForce *= 0.5 * -eePenetration(2) * alpha_ / (1. + TRAIT::fabs(-eePenetration(2) * alpha_)) + 0.5;
+ return;
+ default:
+ throw std::runtime_error("undefined smoothing function");
+ }
+ }
+
+ /*!
+ * \brief computes the damper force \f$ \lambda = d \dot{x} \f$
+ * @param force force to be computed
+ * @param eePenetration endeffector penetration of the surface
+ * @param eeVelocity endeffector velocity
+ */
+ void computeDamperForce(EEForceLinear& force, const Vector3s& eePenetration, const Velocity3S& eeVelocity)
+ {
+ force = -d_ * eeVelocity.toImplementation();
+ }
+
+ void computeNormalSpring(EEForceLinear& force, const SCALAR& p_N, const SCALAR& p_dot_N)
+ {
+ if (alpha_n_ > SCALAR(0))
+ {
+ force(2) += k_ * TRAIT::exp(-alpha_n_ * p_N);
+ }
+ else if (p_N <= SCALAR(0))
+ {
+ force(2) -= k_ * p_N;
+ }
+ }
+
+
+ //! Instance of the kinematics
+ std::shared_ptr<Kinematics> kinematics_;
+
+ //! Type of velocity smoothing
+ VELOCITY_SMOOTHING smoothing_;
+
+ SCALAR k_; //!< spring constant
+ SCALAR d_; //!< damper constant
+ SCALAR alpha_; //!< velocity smoothing coefficient
+ SCALAR alpha_n_; //!< normal force smoothing coefficient
+ SCALAR zOffset_; //!< vertical offset of the contact pane
+
+ ActiveMap EEactive_; //!< stores which endeffectors are active, i.e. can make contact
+};
+}
+}
diff --git a/ct_rbd/include/ct/rbd/rbd-prespec.h b/ct_rbd/include/ct/rbd/rbd-prespec.h
new file mode 100644
index 0000000..58f2d69
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/rbd-prespec.h
@@ -0,0 +1,27 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#ifndef INCLUDE_CT_RBD_RBD_H_
+#define INCLUDE_CT_RBD_RBD_H_
+
+#include <ct/core/core-prespec.h>
+#include <ct/optcon/optcon-prespec.h>
+
+// declarations
+#include "Internal"
+#include "Common"
+#include "State"
+#include "Robot"
+#include "Systems"
+#include "Nloc"
+
+/*!
+ * \warning{do not include implementation files in rbd-prespec.h}
+ */
+
+// keep standard header guard (easy debugging)
+// header guard is identical to the one in rbd.h
+#endif /* INCLUDE_CT_RBD_RBD_H_ */
diff --git a/ct_rbd/include/ct/rbd/rbd.h b/ct_rbd/include/ct/rbd/rbd.h
new file mode 100644
index 0000000..1b96fb3
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/rbd.h
@@ -0,0 +1,30 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#ifndef INCLUDE_CT_RBD_RBD_H_
+#define INCLUDE_CT_RBD_RBD_H_
+
+#include <ct/optcon/optcon.h>
+
+// declarations
+#include "Internal"
+#include "Common"
+#include "State"
+#include "Robot"
+#include "Systems"
+#include "Nloc"
+
+// implementations
+#include "Internal-impl"
+#include "Common-impl"
+#include "State-impl"
+#include "Robot-impl"
+#include "Systems-impl"
+#include "Nloc-impl"
+
+// keep standard header guard (easy debugging)
+// header guard is identical to the one in rbd-prespec.h
+#endif /* INCLUDE_CT_RBD_RBD_H_ */
diff --git a/ct_rbd/include/ct/rbd/robot/Dynamics.h b/ct_rbd/include/ct/rbd/robot/Dynamics.h
new file mode 100644
index 0000000..ae1100a
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/Dynamics.h
@@ -0,0 +1,329 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <memory>
+#include <ct/core/systems/continuous_time/ControlledSystem.h>
+#include "kinematics/RBDDataMap.h"
+#include "Kinematics.h"
+#include "ProjectedDynamics.h"
+#include "state/JointAcceleration.h"
+#include "state/JointState.h"
+#include "state/RBDAcceleration.h"
+#include "state/RBDState.h"
+#include "state/RigidBodyAcceleration.h"
+#include "control/SelectionMatrix.h"
+
+#define ENABLE_FIX_BASE \
+ template <bool B = FB> \
+ typename std::enable_if<!B, void>::type
+#define ENABLE_FIX_BASE_IMPL \
+ template <bool B> \
+ typename std::enable_if<!B, void>::type
+#define ENABLE_FLOAT_BASE \
+ template <bool B = FB> \
+ typename std::enable_if<B, void>::type
+#define ENABLE_FLOAT_BASE_IMPL \
+ template <bool B> \
+ typename std::enable_if<B, void>::type
+
+
+namespace ct {
+namespace rbd {
+
+/**
+ * @brief This class implements the equations of motion of a Rigid Body System
+ * @tparam RBD The rbd container class
+ * @tparam NEE The number of endeffectors
+ */
+template <class RBD, size_t NEE>
+class Dynamics
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef RBD ROBCOGEN;
+ typedef typename ROBCOGEN::SCALAR SCALAR;
+
+ static const bool FB = ROBCOGEN::TRAIT::floating_base;
+
+ static const size_t NJOINTS = RBD::NJOINTS;
+ static const size_t NLINKS = RBD::NLINKS;
+
+
+ // NSTATE either includes the base or is just the joint state
+ static const size_t NSTATE = FB * tpl::RBDState<NJOINTS, SCALAR>::NSTATE + (1 - FB) * 2 * NJOINTS;
+ static const size_t N_EE = NEE;
+
+ //temporary definitions, ideally this should come from RBD
+ typedef Eigen::Matrix<SCALAR, NJOINTS, 1> control_vector_t;
+ typedef Eigen::Matrix<SCALAR, NSTATE, 1> state_vector_t;
+ typedef Eigen::Matrix<SCALAR, 6, 1> Vector6d_t;
+ typedef Vector6d_t ForceVector_t;
+
+ typedef tpl::RBDState<NJOINTS, SCALAR> RBDState_t;
+ typedef tpl::RBDAcceleration<NJOINTS, SCALAR> RBDAcceleration_t;
+ typedef tpl::JointState<NJOINTS, SCALAR> JointState_t;
+ typedef tpl::JointAcceleration<NJOINTS, SCALAR> JointAcceleration_t;
+ typedef tpl::RigidBodyAcceleration<SCALAR> RigidBodyAcceleration_t;
+
+ // currently assumes our input is the joint dimension and we only affect acceleration
+ typedef SelectionMatrix<NJOINTS, NSTATE / 2, SCALAR> SelectionMatrix_t;
+
+ // From the container
+ typedef typename RBD::LinkForceMap ExtLinkForces_t;
+
+ typedef RBDDataMap<bool, NEE> EE_in_contact_t;
+
+ typedef Kinematics<RBD, NEE> Kinematics_t;
+
+ /**
+ * @brief The constructor
+ * @param[in] kinematics The kinematics of the RBD
+ */
+ Dynamics(typename Kinematics_t::Ptr_t kinematics = typename Kinematics_t::Ptr_t(new Kinematics_t()))
+ : S_(FB), kinematics_(kinematics), p_dynamics_(kinematics)
+ {
+ }
+
+ Dynamics(const Dynamics& other) : S_(other.S_), kinematics_(other.kinematics_->clone()), p_dynamics_(kinematics_) {}
+ virtual ~Dynamics(){};
+
+
+ /**
+ * @brief Compute forward dynamics of a fixed-base RBD system under external
+ * forces
+ * @param[in] x The JointState state
+ * @param[in] u The control vector
+ * @param[in] force The external forces vector
+ * @param[out] qdd The joints acceleration
+ */
+ ENABLE_FIX_BASE FixBaseForwardDynamics(const JointState_t& x,
+ const control_vector_t& u,
+ ExtLinkForces_t& force,
+ JointAcceleration_t& qdd);
+
+ /**
+ * @brief Compute forward dynamics of a fixed-base RBD system, NO contact forces
+ * forces
+ * @param[in] x The RBD state
+ * @param[in] u The control vector
+ * @param[out] qdd The Joints acceleration
+ */
+ ENABLE_FIX_BASE FixBaseForwardDynamics(const JointState_t& x, const control_vector_t& u, JointAcceleration_t& qdd)
+ {
+ ExtLinkForces_t force(Eigen::Matrix<SCALAR, 6, 1>::Zero());
+ FixBaseForwardDynamics(x, u, force, qdd);
+ }
+
+ /**
+ * @brief Computes Inverse dynamics of a fixed-base system under external
+ * forces.
+ * @param[in] x the current state of the robot
+ * @param[in] qdd the Joints acceleration
+ * @param[in] force the external forces vector
+ * @param[out] u The control vector
+ */
+ ENABLE_FIX_BASE FixBaseID(const JointState_t& x,
+ const JointAcceleration_t& qdd,
+ const ExtLinkForces_t& force,
+ control_vector_t& u);
+
+ /**
+ * @brief Computes Inverse dynamics of a fixed-base system without external
+ * forces.
+ * @param[in] x the current state of the robot
+ * @param[in] qdd the Joints acceleration
+ * @param[out] u The control vector
+ */
+ ENABLE_FIX_BASE FixBaseID(const JointState_t& x, const JointAcceleration_t& qdd, control_vector_t& u);
+
+ /**
+ * @brief Compute forward dynamics for an floating-base RBD system under external
+ * forces
+ * @param[in] x The RBD state
+ * @param[in] u The control vector
+ * @param[in] link_forces The external forces vector
+ * @param[out] xd The RBD state derivative
+ */
+ ENABLE_FLOAT_BASE FloatingBaseForwardDynamics(const RBDState_t& x,
+ const control_vector_t& u,
+ const ExtLinkForces_t& link_forces,
+ RBDAcceleration_t& xd);
+
+ /**
+ * @brief Computes Inverse dynamics of a floating-base system under external
+ * forces.
+ * @param[in] x the RBDstate
+ * @param[in] qdd the joints acceleration
+ * @param[in] force the external forces vector
+ * @param[out] u The control vector
+ * @param[out] base_a The base state derivative
+ */
+ ENABLE_FLOAT_BASE FloatingBaseID(const RBDState_t& x,
+ const JointAcceleration_t& qdd,
+ const ExtLinkForces_t& force,
+ control_vector_t& u,
+ RigidBodyAcceleration_t& base_a);
+
+ /**
+ * @brief Computes the inverse dynamics of a floating-base fully-actuated
+ * system
+ * @param[in] x The RBDState
+ * @param[in] base_a The base acceleration
+ * @param[in] qdd The joint acceleration
+ * @param[in] force The external forces vector
+ * @param[out] base_w The base wrench
+ * @param[out] u The control vector
+ */
+ ENABLE_FLOAT_BASE FloatingBaseFullyActuatedID(const RBDState_t& x,
+ const RigidBodyAcceleration_t& base_ac,
+ const JointAcceleration_t& qdd,
+ const ExtLinkForces_t& force,
+ ForceVector_t& base_w,
+ control_vector_t& u);
+
+
+ /**
+ * @brief Computes Projected Forward Dynamics of a floating-base system with contact constraints
+ * @param[in] ee_contact the EE contact configuration (EEDataMap<bool>)
+ * @param[in] x the RBD state
+ * @param[in] u the control vector
+ * @param[out] rbd_a the RBD acceleration
+ */
+ ENABLE_FLOAT_BASE ProjectedForwardDynamics(const EE_in_contact_t& ee_contact,
+ const RBDState_t& x,
+ const control_vector_t& u,
+ RBDAcceleration_t& rbd_a)
+ {
+ // optional, for caching
+ ee_contact_ = ee_contact;
+ p_dynamics_.setContactConfiguration(ee_contact);
+
+ p_dynamics_.ProjectedForwardDynamics(x, u, rbd_a);
+ }
+
+ ENABLE_FLOAT_BASE ProjectedInverseDynamics(const EE_in_contact_t& ee_contact,
+ const RBDState_t& x,
+ const RBDAcceleration_t& rbd_a,
+ control_vector_t& u)
+ {
+ // optional, for caching
+ ee_contact_ = ee_contact;
+ p_dynamics_.setContactConfiguration(ee_contact);
+ p_dynamics_.ProjectedInverseDynamics(x, rbd_a, u);
+ }
+
+ ENABLE_FLOAT_BASE ProjectedInverseDynamicsNoGravity(const EE_in_contact_t& ee_contact,
+ const RBDState_t& x,
+ const RBDAcceleration_t& rbd_a,
+ control_vector_t& u)
+ {
+ // optional, for caching
+ ee_contact_ = ee_contact;
+ p_dynamics_.setContactConfiguration(ee_contact);
+ p_dynamics_.ProjectedInverseDynamicsNoGravity(x, rbd_a, u);
+ }
+
+ Kinematics_t& kinematics() { return *kinematics_; }
+ const Kinematics_t& kinematics() const { return *kinematics_; }
+ typename Kinematics_t::Ptr_t& kinematicsPtr() { return kinematics_; }
+ const typename Kinematics_t::Ptr_t& kinematicsPtr() const { return kinematics_; }
+ SelectionMatrix_t& S() { return S_; }
+ const SelectionMatrix_t& S() const { return S_; }
+private:
+ SelectionMatrix_t S_;
+
+ EE_in_contact_t ee_contact_ = false;
+
+ typename Kinematics_t::Ptr_t kinematics_; /*!< The RBD kinematics */
+ ProjectedDynamics<RBD, NEE> p_dynamics_; /*!< The Projected Dynamics */
+
+protected:
+};
+
+
+template <class RBD, size_t NEE>
+ENABLE_FIX_BASE_IMPL Dynamics<RBD, NEE>::FixBaseForwardDynamics(const JointState_t& x,
+ const control_vector_t& u,
+ ExtLinkForces_t& force,
+ JointAcceleration_t& qdd)
+{
+ kinematics_->robcogen().forwardDynamics().fd(qdd.getAcceleration(), x.getPositions(), x.getVelocities(), u, force);
+}
+
+template <class RBD, size_t NEE>
+ENABLE_FIX_BASE_IMPL Dynamics<RBD, NEE>::FixBaseID(const JointState_t& x,
+ const JointAcceleration_t& qdd,
+ const ExtLinkForces_t& force,
+ control_vector_t& u)
+{
+ kinematics_->robcogen().inverseDynamics().id(u, x.getPositions(), x.getVelocities(), qdd.getAcceleration(), force);
+}
+
+template <class RBD, size_t NEE>
+ENABLE_FIX_BASE_IMPL Dynamics<RBD, NEE>::FixBaseID(const JointState_t& x,
+ const JointAcceleration_t& qdd,
+ control_vector_t& u)
+{
+ ExtLinkForces_t force(Eigen::Matrix<SCALAR, 6, 1>::Zero());
+ kinematics_->robcogen().inverseDynamics().id(u, x.getPositions(), x.getVelocities(), qdd.getAcceleration(), force);
+}
+
+template <class RBD, size_t NEE>
+ENABLE_FLOAT_BASE_IMPL Dynamics<RBD, NEE>::FloatingBaseForwardDynamics(const RBDState_t& x,
+ const control_vector_t& u,
+ const ExtLinkForces_t& l_forces,
+ RBDAcceleration_t& xd)
+{
+ Vector6d_t b_accel;
+
+ kinematics_->robcogen().forwardDynamics().fd(xd.joints().getAcceleration(), b_accel, x.baseVelocities().getVector(),
+ x.basePose().computeGravityB6D(), x.joints().getPositions(), x.joints().getVelocities(), u, l_forces);
+
+ xd.base().fromVector6d(b_accel);
+}
+
+template <class RBD, size_t NEE>
+ENABLE_FLOAT_BASE_IMPL Dynamics<RBD, NEE>::FloatingBaseID(const RBDState_t& x,
+ const JointAcceleration_t& qdd,
+ const ExtLinkForces_t& l_forces,
+ control_vector_t& u,
+ RigidBodyAcceleration_t& base_a)
+{
+ Vector6d_t b_accel;
+
+ kinematics_->robcogen().inverseDynamics().id(u,
+ b_accel, //Output
+ x.basePose().computeGravityB6D(), x.baseVelocities().getVector(), x.joints().getPositions(),
+ x.joints().getVelocities(), qdd.getAcceleration(), l_forces);
+
+ base_a.fromVector6d(b_accel);
+}
+
+template <class RBD, size_t NEE>
+ENABLE_FLOAT_BASE_IMPL Dynamics<RBD, NEE>::FloatingBaseFullyActuatedID(const RBDState_t& x,
+ const RigidBodyAcceleration_t& base_a,
+ const JointAcceleration_t& qdd,
+ const ExtLinkForces_t& l_forces,
+ ForceVector_t& base_w,
+ control_vector_t& u)
+{
+ Vector6d_t b_accel;
+
+ kinematics_->robcogen().inverseDynamics().id_fully_actuated(base_w, u, x.basePose().computeGravityB6D(),
+ x.baseVelocities().getVector(), b_accel, x.joints().getPositions(), x.joints().getVelocities(),
+ qdd.getAcceleration(), l_forces);
+}
+
+#undef ENABLE_FIX_BASE
+#undef ENABLE_FIX_BASE_IMPL
+#undef ENABLE_FLOAT_BASE
+#undef ENABLE_FLOAT_BASE_IMPL
+
+} // namespace rbd
+} // namespace ct
diff --git a/ct_rbd/include/ct/rbd/robot/Kinematics.h b/ct_rbd/include/ct/rbd/robot/Kinematics.h
new file mode 100644
index 0000000..64a64b1
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/Kinematics.h
@@ -0,0 +1,378 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <unordered_map>
+
+#include <ct/rbd/common/SpatialForceVector.h>
+#include <ct/rbd/state/RBDState.h>
+
+#include "kinematics/EndEffector.h"
+#include "kinematics/FloatingBaseTransforms.h"
+#include "kinematics/InverseKinematicsBase.h"
+
+namespace ct {
+namespace rbd {
+
+/**
+ * \brief A general class for computing Kinematic properties
+ *
+ * This class implements useful Kinematic quantities. It wraps RobCoGen to
+ * have access to efficient transforms and jacobians.
+ */
+template <class RBD, size_t N_EE>
+class Kinematics
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ Kinematics(std::shared_ptr<RBD> rbdContainer = std::shared_ptr<RBD>(new RBD()))
+ : rbdContainer_(rbdContainer), floatingBaseTransforms_(rbdContainer_)
+ {
+ initEndeffectors(endEffectors_);
+ }
+
+ Kinematics(const Kinematics<RBD, N_EE>& other)
+ : rbdContainer_(new RBD()), endEffectors_(other.endEffectors_), floatingBaseTransforms_(rbdContainer_)
+ {
+ }
+
+ virtual ~Kinematics(){};
+
+ Kinematics<RBD, N_EE>* clone() const { return new Kinematics<RBD, N_EE>(*this); }
+ static const size_t NUM_EE = N_EE;
+ static const size_t NJOINTS = RBD::NJOINTS;
+ static const size_t NLINKS = RBD::NLINKS;
+
+ using Ptr_t = std::shared_ptr<Kinematics<RBD, N_EE>>;
+
+ using ROBCOGEN = RBD;
+ using SCALAR = typename ROBCOGEN::SCALAR;
+
+ using HomogeneousTransform = typename ROBCOGEN::HomogeneousTransform;
+ using HomogeneousTransforms = typename ROBCOGEN::HomogeneousTransforms;
+ using ForceTransform = typename ROBCOGEN::ForceTransform;
+ using Jacobian = typename ROBCOGEN::Jacobian;
+ using Jacobians = typename ROBCOGEN::Jacobians;
+ using Vector3Tpl = Eigen::Matrix<SCALAR, 3, 1>;
+ using Matrix3Tpl = Eigen::Matrix<SCALAR, 3, 3>;
+ using Position3Tpl = kindr::Position<SCALAR, 3>;
+ using Velocity3Tpl = kindr::Velocity<SCALAR, 3>;
+ using QuaterionTpl = kindr::RotationQuaternion<SCALAR>;
+ using RigidBodyPoseTpl = tpl::RigidBodyPose<SCALAR>;
+ using EEForce = SpatialForceVector<SCALAR>;
+ using EEForceLinear = Vector3Tpl;
+
+ void initEndeffectors(std::array<EndEffector<NJOINTS, SCALAR>, NUM_EE>& endeffectors)
+ {
+ for (size_t i = 0; i < NUM_EE; i++)
+ {
+ endeffectors[i].setLinkId(ROBCOGEN::UTILS::eeIdToLinkId(i));
+ }
+ }
+
+ /**
+ * \brief Get an end-effector
+ * @param id end-effector id
+ * @return
+ */
+ EndEffector<NJOINTS, SCALAR>& getEndEffector(size_t id) { return endEffectors_[id]; };
+ /**
+ * \brief Set an end-effector
+ * @param id end-effector id
+ * @param ee end-effector
+ */
+ void setEndEffector(size_t id, const EndEffector<NJOINTS, SCALAR>& ee){};
+
+ Jacobian getJacobianById(size_t linkId)
+ {
+ throw std::runtime_error("getJacobian not implemented");
+ return RigidBodyPoseTpl();
+ };
+
+ FloatingBaseTransforms<RBD>& floatingBaseTransforms()
+ {
+ throw std::runtime_error("floating base transforms not implemented");
+ return floatingBaseTransforms_;
+ };
+ FloatingBaseTransforms<RBD>& floatingBaseTransforms() const
+ {
+ throw std::runtime_error("floating base transforms not implemented");
+ return floatingBaseTransforms_;
+ };
+
+ const HomogeneousTransforms& transforms() const { return robcogen().homogeneousTransforms(); }
+ HomogeneousTransforms& transforms() { return robcogen().homogeneousTransforms(); }
+ const Jacobians& jacobians() const { return robcogen().jacobians(); }
+ Jacobians& jacobians() { return robcogen().jacobians(); }
+ // RigidBodyPoseTpl<SCALAR> getLinkPoseInWorld(size_t linkId, const typename JointStateTpl<NJOINTS,
+ // SCALAR>::Position& jointPosition, const RigidBodyPoseTpl<SCALAR>& basePose) {
+ // throw std::runtime_error("not implemented");
+ // return getHomogeneousTransformBaseLinkById(linkId, jointPosition);
+ // };
+
+ Velocity3Tpl getEEVelocityInBase(size_t eeId, const tpl::RBDState<NJOINTS, SCALAR>& rbdState)
+ {
+ Velocity3Tpl eeVelocityBase;
+ eeVelocityBase.toImplementation() =
+ (robcogen().getJacobianBaseEEbyId(eeId, rbdState.jointPositions()) * rbdState.jointVelocities())
+ .template bottomRows<3>();
+
+ // add translational velocity induced by linear base motion
+ eeVelocityBase += rbdState.base().velocities().getTranslationalVelocity();
+
+ // add translational velocity induced by angular base motion
+ eeVelocityBase += rbdState.base().velocities().getRotationalVelocity().cross(
+ getEEPositionInBase(eeId, rbdState.jointPositions()));
+
+ return eeVelocityBase;
+ }
+
+ Velocity3Tpl getEEVelocityInWorld(size_t eeId, const tpl::RBDState<NJOINTS, SCALAR>& rbdState)
+ {
+ Velocity3Tpl eeVelocityBase = getEEVelocityInBase(eeId, rbdState);
+ return rbdState.base().pose().rotateBaseToInertia(eeVelocityBase);
+ }
+
+ /*!
+ * Computes the forward kinematics for the end-effector position and expresses the end-effector position in robot
+ * base coordinates.
+ * @param eeID unique identifier of the end-effector in question
+ * @param jointPosition current robot joint positions
+ * @return the current end-effector position in base coordinates
+ *
+ * * @todo integrate this into getEEPoseInBase
+ *
+ */
+ Position3Tpl getEEPositionInBase(size_t eeID,
+ const typename tpl::JointState<NJOINTS, SCALAR>::Position& jointPosition)
+ {
+ return robcogen().getEEPositionInBase(eeID, jointPosition);
+ }
+
+ /*!
+ * Computes the forward kinematics for the end-effector position and expresses the end-effector pose in robot base
+ * coordinates.
+ * @param eeID unique identifier of the end-effector in question
+ * @param jointPosition current robot joint positions
+ * @return the current end-effector pose in base coordinates
+ */
+ RigidBodyPoseTpl getEEPoseInBase(size_t eeID,
+ const typename tpl::JointState<NJOINTS, SCALAR>::Position& jointPosition)
+ {
+ return robcogen().getEEPoseInBase(eeID, jointPosition);
+ }
+
+ /*!
+ * compute the forward kinematics and return a rotation matrix specifying the ee-rotation w.r.t. the base frame
+ */
+ Matrix3Tpl getEERotInBase(size_t eeID, const typename tpl::JointState<NJOINTS, SCALAR>::Position& jointPosition)
+ {
+ return robcogen().getEERotInBase(eeID, jointPosition);
+ }
+
+ /*!
+ * Computes the forward kinematics for the end-effector position and expresses the end-effector position in world
+ * coordinates
+ * @param eeID unique identifier of the end-effector in question
+ * @param basePose current robot base pose
+ * @param jointPosition current robot joint positions
+ * @return the current end-effector position in world coordinates
+ *
+ * @todo integrate this into getEEPoseInWorld
+ */
+ Position3Tpl getEEPositionInWorld(size_t eeID,
+ const RigidBodyPoseTpl& basePose,
+ const typename tpl::JointState<NJOINTS, SCALAR>::Position& jointPosition)
+ {
+ // vector from base to endeffector expressed in base frame
+ Position3Tpl B_x_EE = getEEPositionInBase(eeID, jointPosition);
+
+ // vector from base to endeffector expressed in world frame
+ Position3Tpl W_x_EE = basePose.template rotateBaseToInertia(B_x_EE);
+
+ // vector from origin to endeffector = vector from origin to base + vector from base to endeffector
+ return basePose.position() + W_x_EE;
+ }
+
+ //! get the end-effector pose in world coordinates
+ RigidBodyPoseTpl getEEPoseInWorld(size_t eeID,
+ const RigidBodyPoseTpl& basePose,
+ const typename tpl::JointState<NJOINTS, SCALAR>::Position& jointPosition)
+ {
+ // ee pose in base coordinates
+ RigidBodyPoseTpl B_x_EE = getEEPoseInBase(eeID, jointPosition);
+
+ // position rotated into world frame
+ Position3Tpl W_p_EE = basePose.template rotateBaseToInertia(B_x_EE.position());
+
+ // orientation rotated into world frame
+ QuaterionTpl B_q_EE = B_x_EE.getRotationQuaternion();
+ QuaterionTpl W_q_EE = basePose.template rotateBaseToInertiaQuaternion(B_q_EE);
+
+ return RigidBodyPoseTpl(W_q_EE, basePose.position() + W_p_EE);
+ }
+
+ //! get the end-effector rotation matrix expressed in world coordinates
+ Matrix3Tpl getEERotInWorld(size_t eeID,
+ const RigidBodyPoseTpl& basePose,
+ const typename tpl::JointState<NJOINTS, SCALAR>::Position& jointPosition)
+ {
+ // ee rotation matrix in base coordinates
+ Matrix3Tpl B_R_EE = getEERotInBase(eeID, jointPosition);
+
+ // ee rotation matriix in world
+ return basePose.template rotateBaseToInertiaMat(B_R_EE);
+ }
+
+ void addIKSolver(const std::shared_ptr<InverseKinematicsBase<NJOINTS, SCALAR>>& solver,
+ size_t eeID,
+ size_t solverID = 0)
+ {
+ if (solverID >= 100)
+ throw std::runtime_error("Solver ID must be less than 100.");
+
+ size_t hash = eeID * 100 + solverID;
+ if (ikSolvers_.find(hash) != ikSolvers_.end())
+ throw std::runtime_error("Solver with the same eeID and solverID already present.");
+ ikSolvers_[hash] = solver;
+ }
+
+ std::shared_ptr<InverseKinematicsBase<NJOINTS, SCALAR>> getIKSolver(const size_t eeID,
+ const size_t solverID = 0) const
+ {
+ if (solverID >= 100)
+ throw std::runtime_error("Solver ID must be less than 100.");
+ size_t hash = eeID * 100 + solverID;
+ return ikSolvers_[hash];
+ }
+
+ /**
+ * \brief Transforms a force applied at an end-effector and expressed in the world into the link frame the EE is
+ * rigidly connected to.
+ * @param W_force Force expressed in world coordinates
+ * @param basePose Pose of the base (in the world)
+ * @param jointPosition Joint angles
+ * @param eeId ID of the end-effector
+ * @return
+ */
+ EEForce mapForceFromWorldToLink3d(const Vector3Tpl& W_force,
+ const RigidBodyPoseTpl& basePose,
+ const typename tpl::JointState<NJOINTS, SCALAR>::Position& jointPosition,
+ size_t eeId)
+ {
+ EEForce eeForce(EEForce::Zero());
+ eeForce.force() = W_force;
+
+ return mapForceFromWorldToLink(eeForce, basePose, jointPosition, eeId);
+ }
+
+ /**
+ * \brief Transforms a force applied at an end-effector and expressed in the world into the link frame the EE is
+ * rigidly connected to.
+ * @param W_force Force expressed in world coordinates
+ * @param basePose Pose of the base (in the world)
+ * @param jointPosition Joint angles
+ * @param eeId ID of the end-effector
+ * @return
+ */
+ EEForce mapForceFromWorldToLink(const EEForce& W_force,
+ const RigidBodyPoseTpl& basePose,
+ const typename tpl::JointState<NJOINTS, SCALAR>::Position& jointPosition,
+ size_t eeId)
+ {
+ // get endeffector position in world
+ Position3Tpl B_x_EE = getEEPositionInBase(eeId, jointPosition);
+
+ return mapForceFromWorldToLink(W_force, basePose, jointPosition, B_x_EE, eeId);
+ }
+
+ /**
+ * \brief Transforms a force applied at an end-effector and expressed in the world into the link frame the EE is
+ * rigidly connected to.
+ * @param W_force Force expressed in world coordinates
+ * @param basePose Pose of the base (in the world)
+ * @param jointPosition Joint angles
+ * @param B_x_EE Position of the end effector in the base
+ * @param eeId ID of the end-effector
+ * @return
+ */
+ EEForce mapForceFromWorldToLink(const EEForce& W_force,
+ const RigidBodyPoseTpl& basePose,
+ const typename tpl::JointState<NJOINTS, SCALAR>::Position& jointPosition,
+ const Position3Tpl& B_x_EE,
+ size_t eeId)
+ {
+ // get the link that the EE is attached to
+ size_t linkId = getEndEffector(eeId).getLinkId();
+
+ // transform the force/torque to an equivalent force/torque in the base using a lever-arm for the torque
+ EEForce B_force;
+
+ B_force.force() = basePose.template rotateInertiaToBase<Vector3Tpl>(W_force.force());
+ B_force.torque() = B_x_EE.toImplementation().cross(B_force.force()) +
+ basePose.template rotateInertiaToBase<Vector3Tpl>(W_force.torque());
+
+ // transform force to link on which endeffector sits on
+ return EEForce(robcogen().getForceTransformLinkBaseById(linkId, jointPosition) * B_force);
+ }
+
+ /**
+ * \brief Transforms a force applied at an end-effector expressed in an arbitrary (end-effector)
+ * frame into the link frame the EE is rigidly connected to.
+ *
+ * This function does not assume any position or orientation of the end-effector. Therefore,
+ * the user must specify the orientation of the end-effector in the base. There are two common
+ * choices: use zero rotation to assume forces/torques are expressed in the base. Or use the
+ * base orientation to assume the end-effector forces/torques are expressed in the world. For
+ * this variant, also see mapForceFromWorldToLink().
+ *
+ * NOTE: Even if zero or base rotation is assumed, you have to pass the correct position of the
+ * end-effector expressed in the base as part of T_B_EE! Do NOT pass the base pose directly here!
+ *
+ * @param EE_force 6D torque/force vector expressed in the EE frame
+ * @param T_B_EE transform from end-effector to base
+ * @param jointPosition joint angles
+ * @param eeId ID of the end-effector
+ * @return
+ */
+ EEForce mapForceFromEEToLink(const EEForce& EE_force,
+ const RigidBodyPoseTpl& T_B_EE,
+ const typename tpl::JointState<NJOINTS, SCALAR>::Position& jointPosition,
+ size_t eeId)
+ {
+ // get the link that the EE is attached to
+ size_t linkId = getEndEffector(eeId).getLinkId();
+
+ // transform the force/torque to an equivalent force/torque in the base using a lever-arm for the torque
+ EEForce B_force;
+
+ // convenience accessors
+ Vector3Tpl B_x_EE = T_B_EE.position().toImplementation();
+
+ // rotate from the end-effector frame to the inertia frame
+ B_force.force() = T_B_EE.template rotateBaseToInertia<Vector3Tpl>(EE_force.force());
+
+ // build the cross product and add the torque (which first is rotated from the end-effector to the base)
+ B_force.torque() =
+ B_x_EE.cross(B_force.force()) + T_B_EE.template rotateBaseToInertia<Vector3Tpl>(EE_force.torque());
+
+ // transform force to link on which endeffector sits on
+ return EEForce(robcogen().getForceTransformLinkBaseById(linkId, jointPosition) * B_force);
+ };
+
+ RBD& robcogen() { return *rbdContainer_; }
+private:
+ std::shared_ptr<RBD> rbdContainer_;
+ std::array<EndEffector<NJOINTS, SCALAR>, N_EE> endEffectors_;
+ FloatingBaseTransforms<RBD> floatingBaseTransforms_;
+
+ std::unordered_map<size_t, std::shared_ptr<InverseKinematicsBase<NJOINTS, SCALAR>>> ikSolvers_;
+};
+
+} /* namespace rbd */
+} /* namespace ct */
diff --git a/ct_rbd/include/ct/rbd/robot/ProjectedDynamics.h b/ct_rbd/include/ct/rbd/robot/ProjectedDynamics.h
new file mode 100644
index 0000000..e1184a5
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/ProjectedDynamics.h
@@ -0,0 +1,322 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+
+#include "jacobian/ConstraintJacobian.h"
+#include "kinematics/RBDDataMap.h"
+#include "Kinematics.h"
+#include "state/JointAcceleration.h"
+#include "state/RBDAcceleration.h"
+#include "state/RBDState.h"
+#include "state/RigidBodyAcceleration.h"
+#include "state/RigidBodyPose.h"
+#include "state/RigidBodyState.h"
+
+namespace ct {
+namespace rbd {
+
+template <class RBD, size_t NEE>
+class ProjectedDynamics
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef typename RBD::SCALAR Scalar;
+
+ static const size_t NJOINTS = RBD::NJOINTS;
+ static const size_t CONTROL_DIM = RBD::NJOINTS; //this is very big assumption!
+ static const size_t NDOF = RBDState<NJOINTS>::NDOF;
+ static const size_t MAX_JAC_SIZE = 3 * NEE;
+
+ // this typedefs should live somewhere else
+ typedef Eigen::Matrix<Scalar, NDOF, 1> g_coordinate_vector_t;
+ typedef Eigen::Matrix<Scalar, CONTROL_DIM, 1> control_vector_t;
+ typedef Eigen::Matrix<Scalar, NJOINTS, 1> joint_vector_t;
+ typedef Eigen::Matrix<Scalar, NDOF, NDOF> inertia_matrix_t;
+ typedef Eigen::Matrix<Scalar, 6, 1> ForceVector_t;
+ typedef Eigen::Matrix<Scalar, CONTROL_DIM, NDOF> selection_matrix_t;
+
+ typedef RBDDataMap<Eigen::Vector3d, NEE> EE_contact_forces_t;
+ typedef RBDDataMap<bool, NEE> EE_in_contact_t;
+
+ typedef tpl::RBDState<NJOINTS, Scalar> RBDState_t;
+ typedef tpl::RBDAcceleration<NJOINTS, Scalar> RBDAcceleration_t;
+ typedef tpl::JointState<NJOINTS, Scalar> JointState_t;
+ typedef tpl::JointAcceleration<NJOINTS, Scalar> JointAcceleration_t;
+
+ typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;
+ typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorXs;
+
+ /**
+ * @brief The Constructor
+ * @param[in] kyn Robot Kinematics
+ * @param[in] ee_inc The EE Boolean Data Map with the end effectors in contact
+ */
+ ProjectedDynamics(const std::shared_ptr<Kinematics<RBD, NEE>> kyn,
+ const EE_in_contact_t ee_inc = EE_in_contact_t(false));
+
+ ~ProjectedDynamics(){};
+
+ /**
+ * @brief set the End Effector contact configuration
+ * @param[in] eeinc The EE Boolean Data Map of the contact configuration
+ */
+ void setContactConfiguration(const EE_in_contact_t& eeinc)
+ {
+ ee_in_contact_ = eeinc;
+ ResetJacobianStructure();
+ }
+
+ /**
+ * @brief get the End Effector contact configuration
+ * @param[out] eeinc The EE Boolean Data Map of the contact configuration
+ */
+ void getContactConfiguration(EE_in_contact_t& eeinc) { eeinc = ee_in_contact_; }
+ /**
+ * @brief Computes the forward dynamics in the constraint consistent subspace
+ * of the current contact configuration
+ * param[in] x The RBD state
+ * param[in] u The control vector
+ * param[out] qdd The RBD acceleration
+ */
+ void ProjectedForwardDynamics(const RBDState_t& x, const control_vector_t& u, RBDAcceleration_t& qdd)
+ {
+ updatePD(x, u);
+
+ g_coordinate_vector_t qd = x.toCoordinateVelocity();
+
+ g_coordinate_vector_t b = fp_ - hp_ - (Cc_ * qd);
+ // - M J^-1 J_dot * qd +
+ //b = -Cc_ * qd + fp_ - hp_;
+
+ g_coordinate_vector_t y = Mc_.fullPivLu().solve(b);
+
+ //ToDo: better assignment methods
+ qdd.base().fromVector6d(y.template segment<6>(0));
+ qdd.joints().setAcceleration(y.template segment<NJOINTS>(6));
+ }
+
+ /**
+ * @brief Computes the Inverse Dynamics in the constraint consistent subspace
+ * of the current contact configuration
+ * param[in] x The RBD state
+ * param[in] qdd The RBD Acceleration
+ * param[out] u The control vector
+ */
+ void ProjectedInverseDynamics(const RBDState_t& x, const RBDAcceleration_t& qdd, control_vector_t& u)
+ {
+ /// This method is generic for any type of projection, however the current implementation
+ /// of the class only provides the orthogonal projector.
+ /// ToDo: For the orthogonal projection some of these operations are redundant
+
+ updatePD(x, u);
+
+ Eigen::Matrix<Scalar, NDOF, CONTROL_DIM> PSt = P_ * S_.transpose();
+
+ svd_.compute(PSt, Eigen::ComputeThinU | Eigen::ComputeThinV);
+
+ VectorXs sing_values(svd_.matrixV().cols(), 1); // size of E has same size as columns of V
+ sing_values = (svd_.singularValues().array() > 1e-9).select(svd_.singularValues().array().inverse(), 0);
+ selection_matrix_t Pstinv = svd_.matrixV() * sing_values.asDiagonal() * svd_.matrixU().transpose();
+
+ selection_matrix_t PStP = Pstinv * P_;
+
+ g_coordinate_vector_t Mqdd = M_ * qdd.toCoordinateAcceleration();
+
+ u = PStP * (Mqdd + h_);
+ }
+
+ void ProjectedInverseDynamicsNoGravity(const RBDState_t& x, const RBDAcceleration_t& qdd, control_vector_t& u)
+ {
+ /// This method is generic for any type of projection, however the current implementation
+ /// of the class only provides the orthogonal projector.
+ /// ToDo: For the orthogonal projection some of these operations are redundant
+
+ updatePDNoGravity(x, u);
+
+ Eigen::Matrix<Scalar, NDOF, CONTROL_DIM> PSt = P_ * S_.transpose();
+
+ svd_.compute(PSt, Eigen::ComputeThinU | Eigen::ComputeThinV);
+
+ VectorXs sing_values(svd_.matrixV().cols(), 1); // size of E has same size as columns of V
+ sing_values = (svd_.singularValues().array() > 1e-9).select(svd_.singularValues().array().inverse(), 0);
+ selection_matrix_t Pstinv = svd_.matrixV() * sing_values.asDiagonal() * svd_.matrixU().transpose();
+
+ selection_matrix_t PStP = Pstinv * P_;
+
+ g_coordinate_vector_t Mqdd = M_ * qdd.toCoordinateAcceleration();
+
+ u = PStP * (Mqdd + h_);
+ }
+
+ /// @brief compute contact forces from last dynamics call given the last
+ /// call of the dynamics.
+ /// the idea is to allow the user to get the contact forces only if required
+ void computeContactForces(EE_contact_forces_t& lamda);
+
+private:
+ ///@brief Update constraint dynamics terms
+ void updatePD(const RBDState_t& x, const control_vector_t& u);
+
+ void updatePDNoGravity(const RBDState_t& x, const control_vector_t& u);
+
+ ///@brief Update the Projected terms
+ void updateProjectedTerms(const control_vector_t& u);
+
+ /// @brief Update M h & f terms of the dynamics equation
+ void updateDynamicsTerms(const RBDState_t& x, const control_vector_t& u);
+
+ void updateDynamicsTermsNoGravity(const RBDState_t& x, const control_vector_t& u);
+
+ void ResetJacobianStructure();
+
+private:
+ std::shared_ptr<Kinematics<RBD, NEE>> kinematics_; /*!< The RBD kinematics */
+
+ EE_in_contact_t ee_in_contact_; /*!< the contact configuration data map*/
+
+ size_t neec_ = 0; /*!< The number of EE in contact */
+
+ inertia_matrix_t M_; /*!< The inertia matrix */
+ selection_matrix_t S_; /*!< The selection matrix */
+ g_coordinate_vector_t f_; /*!< The input force*/
+ g_coordinate_vector_t h_; /*!< the c and g-forces */
+
+ inertia_matrix_t Mc_; /*!< The constraint inertia matrix */
+ g_coordinate_vector_t fp_; /*!< acting input force*/
+ g_coordinate_vector_t hp_; /*!< acting forces */
+ inertia_matrix_t Cc_; /*!< Internal forces (ToDo: find better name)*/
+
+
+ tpl::ConstraintJacobian<Kinematics<RBD, NEE>, MAX_JAC_SIZE, NJOINTS, Scalar>
+ Jc_; /*!< The Jacobian of the constraint */
+
+ inertia_matrix_t P_; /*!< The Projector */
+ Eigen::JacobiSVD<MatrixXs> svd_;
+};
+
+template <class RBD, size_t NEE>
+ProjectedDynamics<RBD, NEE>::ProjectedDynamics(const std::shared_ptr<Kinematics<RBD, NEE>> kyn,
+ const EE_in_contact_t ee_inc /*= EE_in_Contact_t(false)*/)
+ : kinematics_(kyn), ee_in_contact_(ee_inc)
+{
+}
+
+template <class RBD, size_t NEE>
+void ProjectedDynamics<RBD, NEE>::updatePD(const RBDState_t& x, const control_vector_t& u)
+{
+ // currently using the null space orthogonal projection
+
+ updateDynamicsTerms(x, u);
+ Jc_.updateState(x);
+ updateProjectedTerms(u);
+}
+
+template <class RBD, size_t NEE>
+void ProjectedDynamics<RBD, NEE>::updatePDNoGravity(const RBDState_t& x, const control_vector_t& u)
+{
+ // currently using the null space orthogonal projection
+
+ updateDynamicsTermsNoGravity(x, u);
+ Jc_.updateState(x);
+ updateProjectedTerms(u);
+}
+
+template <class RBD, size_t NEE>
+void ProjectedDynamics<RBD, NEE>::updateDynamicsTermsNoGravity(const RBDState_t& x, const control_vector_t& u)
+{
+ joint_vector_t jForces;
+ ForceVector_t base_w;
+
+ kinematics_->robcogen().inverseDynamics().C_terms_fully_actuated(
+ base_w, jForces, x.baseVelocities().getVector(), x.joints().getPositions(), x.joints().getVelocities());
+
+ M_ = kinematics_->robcogen().jSim().update(x.joints().getPositions());
+
+ // todo replace with selection matrix from kinematics_!
+ S_.template block<CONTROL_DIM, 6>(0, 0).setZero();
+ S_.template block<CONTROL_DIM, NJOINTS>(0, 6).setIdentity();
+
+ h_ << base_w, jForces;
+ f_ << Eigen::Matrix<Scalar, 6, 1>::Zero(), u;
+}
+
+template <class RBD, size_t NEE>
+void ProjectedDynamics<RBD, NEE>::updateDynamicsTerms(const RBDState_t& x, const control_vector_t& u)
+{
+ joint_vector_t jForces, jForces_gravity, jForces_velocity;
+ ForceVector_t base_w, base_w_gravity, base_w_velocity;
+
+
+ // Calls set kinematics->setjointstates
+ kinematics_->robcogen().inverseDynamics().G_terms_fully_actuated(
+ base_w_gravity, jForces_gravity, x.basePose().computeGravityB6D(), x.joints().getPositions());
+
+ kinematics_->robcogen().inverseDynamics().C_terms_fully_actuated(
+ base_w_velocity, jForces_velocity, x.baseVelocities().getVector(), x.joints().getVelocities());
+
+ base_w = base_w_gravity + base_w_velocity;
+ jForces = jForces_gravity + jForces_velocity;
+
+ M_ = kinematics_->robcogen().jSim().update(x.joints().getPositions());
+
+ // todo replace with selection matrix from kinematics_!
+ S_.template block<CONTROL_DIM, 6>(0, 0).setZero();
+ S_.template block<CONTROL_DIM, NJOINTS>(0, 6).setIdentity();
+
+ h_ << base_w, jForces;
+ f_ << Eigen::Matrix<Scalar, 6, 1>::Zero(), u;
+}
+
+template <class RBD, size_t NEE>
+void ProjectedDynamics<RBD, NEE>::updateProjectedTerms(const control_vector_t& u)
+{
+ P_ = Jc_.P();
+
+ /// Constraint Inertia Matrix
+ /// There are various versions, user should decide on this?
+ ///
+ inertia_matrix_t PM = P_ * M_;
+ inertia_matrix_t PMT = PM.transpose();
+ Mc_ = M_ + PM - PMT;
+
+ ///Acting input force
+ fp_ = P_ * S_.transpose() * u;
+
+ ///Acting forces
+ hp_ = P_ * h_;
+
+ ///InternalForces
+ inertia_matrix_t C = Jc_.JdagerSVD() * Jc_.dJdt();
+ Cc_ = M_ * C;
+}
+
+template <class RBD, size_t NEE>
+void ProjectedDynamics<RBD, NEE>::ResetJacobianStructure()
+{
+ Jc_.ee_indices_.clear();
+ Jc_.c_size_ = 0;
+ neec_ = 0;
+
+
+ for (auto eeinc = 0; eeinc < NEE; eeinc++)
+ {
+ if (ee_in_contact_[eeinc])
+ {
+ neec_++;
+ Jc_.c_size_ += 3;
+ Jc_.ee_indices_.push_back(eeinc);
+
+ Jc_.eeInContact_[eeinc] = ee_in_contact_[eeinc];
+ }
+ }
+}
+
+} /* namespace rbd */
+} /* namespace ct */
diff --git a/ct_rbd/include/ct/rbd/robot/RobCoGenContainer.h b/ct_rbd/include/ct/rbd/robot/RobCoGenContainer.h
new file mode 100644
index 0000000..8757406
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/RobCoGenContainer.h
@@ -0,0 +1,214 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <kindr/Core>
+#include <ct/rbd/state/JointState.h>
+
+namespace ct {
+namespace rbd {
+
+/**
+ * \brief Container class containing all robcogen classes
+ */
+template <class RBDTrait, template <typename> class LinkDataMapT, class U>
+class RobCoGenContainer
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ RobCoGenContainer()
+ : homogeneousTransforms_(),
+ motionTransforms_(),
+ forceTransforms_(),
+ jacobians_(),
+ inertiaProperties_(),
+ jSim_(inertiaProperties_, forceTransforms_),
+ forwardDynamics_(inertiaProperties_, motionTransforms_),
+ inverseDynamics_(inertiaProperties_, motionTransforms_){};
+
+ typedef typename RBDTrait::S SCALAR;
+
+ typedef RBDTrait TRAIT;
+
+ typedef U UTILS;
+
+ static const size_t NJOINTS = RBDTrait::joints_count;
+ static const size_t NLINKS = RBDTrait::links_count;
+
+ typedef RobCoGenContainer<RBDTrait, LinkDataMapT, UTILS> specialized_t;
+ typedef std::shared_ptr<specialized_t> Ptr_t;
+
+ typedef typename RBDTrait::HomogeneousTransforms HomogeneousTransforms;
+ typedef typename RBDTrait::MotionTransforms MotionTransforms;
+ typedef typename RBDTrait::ForceTransforms ForceTransforms;
+ typedef typename RBDTrait::Jacobians Jacobians;
+ typedef typename RBDTrait::InertiaProperties InertiaProperties;
+ typedef typename RBDTrait::JSIM JSIM;
+ typedef typename RBDTrait::FwdDynEngine ForwardDynamics;
+ typedef typename RBDTrait::InvDynEngine InverseDynamics;
+ typedef typename RBDTrait::LinkID LinkIdentifiers;
+
+ typedef LinkDataMapT<Eigen::Matrix<SCALAR, 6, 1>> LinkForceMap;
+
+ typedef Eigen::Matrix<SCALAR, 4, 4> HomogeneousTransform;
+ typedef Eigen::Matrix<SCALAR, 6, 6> ForceTransform;
+ typedef Eigen::Matrix<SCALAR, 6, NJOINTS> Jacobian;
+ typedef Eigen::Matrix<SCALAR, 3, 3> Matrix3Tpl;
+
+ typedef kindr::Position<SCALAR, 3> Position3Tpl;
+ typedef Eigen::Matrix<SCALAR, 3, 1> Vector3Tpl;
+
+ using RigidBodyPoseTpl = tpl::RigidBodyPose<SCALAR>;
+
+
+ HomogeneousTransforms& homogeneousTransforms() { return homogeneousTransforms_; };
+ const HomogeneousTransforms& homogeneousTransforms() const { return homogeneousTransforms_; };
+ MotionTransforms& motionTransforms() { return motionTransforms_; };
+ const MotionTransforms& motionTransforms() const { return motionTransforms_; };
+ ForceTransforms& forceTransforms() { return forceTransforms_; };
+ const ForceTransforms& forceTransforms() const { return forceTransforms_; };
+ Jacobians& jacobians() { return jacobians_; };
+ const Jacobians& jacobians() const { return jacobians_; };
+ InertiaProperties& inertiaProperties() { return inertiaProperties_; };
+ const InertiaProperties& inertiaProperties() const { return inertiaProperties_; };
+ JSIM& jSim() { return jSim_; }
+ const JSIM& jSim() const { return jSim_; }
+ ForwardDynamics& forwardDynamics() { return forwardDynamics_; };
+ const ForwardDynamics& forwardDynamics() const { return forwardDynamics_; };
+ InverseDynamics& inverseDynamics() { return inverseDynamics_; };
+ const InverseDynamics& inverseDynamics() const { return inverseDynamics_; };
+ /**
+ * \brief Get a homogeneous transformation from link to base provided a link id
+ *
+ * The homogeneous transform takes a quantity expressed in the link frame and transforms
+ * it into the base frame, i.e. \f$ {_Bx} = T_{BL} {_Lx} \f$
+ * where \f$ T_{BL} \f$ is the transform you obtain from this function.
+ *
+ * @param linkId link id
+ * @param position joint positions (angles)
+ * @return Homogeneous transformation from link to base \f$ T_{BL} \f$
+ */
+ HomogeneousTransform getHomogeneousTransformBaseLinkById(size_t linkId,
+ const typename tpl::JointState<NJOINTS, SCALAR>::Position& jointPosition)
+ {
+ return UTILS::getTransformBaseLinkById(homogeneousTransforms(), linkId, jointPosition);
+ }
+
+ /**
+ * \brief Get a force transformation from link to base provided a link id
+ *
+ * The force transform takes a torque-force vector expressed in the base frame and transforms
+ * it into the link frame, i.e. \f$ {_Lx} = T_{LB} {_Bx} \f$
+ * where \f$ T_{LB} \f$ is the transform you obtain from this function.
+ *
+ * @param linkId link id
+ * @param position joint positions (angles)
+ * @return Force transformation from link to base \f$ T_{LB} \f$
+ */
+ ForceTransform getForceTransformLinkBaseById(size_t linkId,
+ const typename tpl::JointState<NJOINTS, SCALAR>::Position& jointPosition)
+ {
+ return UTILS::getTransformLinkBaseById(forceTransforms(), linkId, jointPosition);
+ }
+
+ /**
+ * \brief Get a homogeneous transformation from base to an endeffector provided an endeffector id
+ *
+ * The homogeneous transform converts a quantity expressed in the base and converts it to the end-effector frame, i.e.
+ * \f$ {_EEx} = T_{EE-B} {_Bx} \f$
+ * The endeffector frame corresponds to the convention used when creating the RobCoGen code.
+ *
+ * @param eeId End-effector ID
+ * @param jointPosition Joint angles/positions
+ * @return Homogeneous transformation from base to endeffector \f$ T_{EE-B} \f$
+ */
+ HomogeneousTransform getHomogeneousTransformBaseEEById(size_t eeId,
+ const typename tpl::JointState<NJOINTS, SCALAR>::Position& jointPosition)
+ {
+ return UTILS::getTransformBaseEEById(homogeneousTransforms(), eeId, jointPosition);
+ }
+
+
+ /*!
+ * \brief Get the end-effector Jacobian expressed in the base frame
+ * @param eeId endeffector ID
+ * @param jointPosition current joint position
+ * @return Jacobian of the endeffector expressed in the base frame
+ */
+ Jacobian getJacobianBaseEEbyId(size_t eeId,
+ const typename tpl::JointState<NJOINTS, SCALAR>::Position& jointPosition)
+ {
+ return UTILS::getJacobianBaseEEbyId(jacobians(), eeId, jointPosition);
+ }
+
+ // /**
+ // * \brief Get a force transformation from base to an endeffector provided an endeffector id
+ // *
+ // * The force transform converts a torque-force vector expressed in the base and converts it to the end-effector frame, i.e.
+ // * \f$ {_EEx} = T_{EE-B} {_Bx} \f$
+ // * The endeffector frame corresponds to the convention used when creating the RobCoGen code.
+ // *
+ // * @param eeId End-effector ID
+ // * @param jointPosition Joint angles/positions
+ // * @return Homogeneous transformation from base to endeffector \f$ T_{EE-B} \f$
+ // */
+ // ForceTransform getForceTransformEEBaseById(size_t eeId, const typename JointState<NJOINTS>::Position& jointPosition);
+
+ /*!
+ * \brief Get the endeffector position expressed in the base frame
+ *
+ * @param eeId endeffector ID
+ * @param jointPosition current joint position
+ * @return position of the endeffector expressed in the base frame
+ */
+ Position3Tpl getEEPositionInBase(size_t eeId,
+ const typename tpl::JointState<NJOINTS, SCALAR>::Position& jointPosition)
+ {
+ return Position3Tpl(getHomogeneousTransformBaseEEById(eeId, jointPosition).template topRightCorner<3, 1>());
+ }
+
+ /*!
+ * \brief Get the endeffector pose expressed in the base frame
+ *
+ * @param eeId endeffector ID
+ * @param jointPosition current joint position
+ * @param storage the type of storage inteded for the pose
+ * @return position of the endeffector expressed in the base frame
+ */
+ RigidBodyPoseTpl getEEPoseInBase(size_t eeId,
+ const typename tpl::JointState<NJOINTS, SCALAR>::Position& jointPosition,
+ typename RigidBodyPoseTpl::STORAGE_TYPE storage = RigidBodyPoseTpl::EULER)
+ {
+ // construct the rigid body pose from a homogeneous transformation matrix
+ RigidBodyPoseTpl pose(getHomogeneousTransformBaseEEById(eeId, jointPosition), storage);
+ return pose;
+ }
+
+ /*!
+ * compute the forward kinematics and return a rotation matrix specifying the ee-rotation w.r.t. the base frame
+ */
+ Matrix3Tpl getEERotInBase(size_t eeId, const typename tpl::JointState<NJOINTS, SCALAR>::Position& jointPosition)
+ {
+ return getHomogeneousTransformBaseEEById(eeId, jointPosition).template topLeftCorner<3, 3>();
+ }
+
+
+private:
+ HomogeneousTransforms homogeneousTransforms_;
+ MotionTransforms motionTransforms_;
+ ForceTransforms forceTransforms_;
+ Jacobians jacobians_;
+ InertiaProperties inertiaProperties_;
+ JSIM jSim_;
+ ForwardDynamics forwardDynamics_;
+ InverseDynamics inverseDynamics_;
+};
+
+
+} // namespace rbd
+} // namespace ct
diff --git a/ct_rbd/include/ct/rbd/robot/actuator/ActuatorDynamics.h b/ct_rbd/include/ct/rbd/robot/actuator/ActuatorDynamics.h
new file mode 100644
index 0000000..022d5ff
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/actuator/ActuatorDynamics.h
@@ -0,0 +1,75 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/rbd/state/JointState.h>
+
+namespace ct {
+namespace rbd {
+
+
+/*!
+ * This class covers the actuator dynamics for a robot, i.e. not the dynamics of a single actuator, but
+ * the dynamics of the collection of all actuators in the system
+ *
+ * \note This class does on purpose not derive from ControlledSystem, as it requires the full robot state
+ *
+ * @tparam state dimensions of all actuators in the system together
+ * @tparam number of joints in the robot
+ * @tparam primitive scalar type, eg. double
+ */
+template <size_t ACT_STATE_DIMS, size_t NJOINTS, typename SCALAR = double>
+class ActuatorDynamics
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ static const size_t ACT_STATE_DIM = ACT_STATE_DIMS;
+ static const size_t ACT_CONTROL_DIM = NJOINTS;
+
+ typedef ct::core::StateVector<ACT_STATE_DIMS, SCALAR> act_state_vector_t;
+ typedef ct::core::ControlVector<NJOINTS, SCALAR> act_control_vector_t;
+
+ ActuatorDynamics(){};
+
+ virtual ~ActuatorDynamics(){};
+
+ virtual ActuatorDynamics<ACT_STATE_DIMS, NJOINTS, SCALAR>* clone() const = 0;
+
+ virtual void computeActuatorDynamics(const ct::rbd::tpl::JointState<NJOINTS, SCALAR>& robotJointState,
+ const ct::core::StateVector<ACT_STATE_DIMS, SCALAR>& actuatorState,
+ const SCALAR& t,
+ const ct::core::ControlVector<NJOINTS, SCALAR>& control,
+ ct::core::StateVector<ACT_STATE_DIMS, SCALAR>& derivative) = 0;
+
+ /**
+ * @brief output equation of the actuator
+ *
+ * Algebraic output equation for the actuator system, translating the current actuator state and the current robot state into
+ * an output control vector.
+ * Example: in an RBD setting, the controlOutput may be the actual joint torques.
+ *
+ * @param robotState current RBD state of the robot
+ * @param actState current state of the actuators
+ * @param controlOutput control output (output side of actuator)
+ */
+ virtual core::ControlVector<NJOINTS, SCALAR> computeControlOutput(
+ const ct::rbd::tpl::JointState<NJOINTS, SCALAR>& robotJointState,
+ const act_state_vector_t& actState) = 0;
+
+ /*!
+ * @brief reconstruct actuator state from a desired control output and robot joint state (e.g. for initialization)
+ * @return the actuator state resulting in the desired control output
+ */
+ virtual ct::core::StateVector<ACT_STATE_DIMS, SCALAR> computeStateFromOutput(
+ const ct::rbd::tpl::JointState<NJOINTS, SCALAR>& refRobotJointState,
+ const core::ControlVector<NJOINTS, SCALAR>& refControl) = 0;
+};
+
+
+} // namespace rbd
+} // namespace ct
diff --git a/ct_rbd/include/ct/rbd/robot/actuator/ActuatorDynamicsSymplectic.h b/ct_rbd/include/ct/rbd/robot/actuator/ActuatorDynamicsSymplectic.h
new file mode 100644
index 0000000..b0792be
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/actuator/ActuatorDynamicsSymplectic.h
@@ -0,0 +1,79 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/rbd/state/JointState.h>
+
+namespace ct {
+namespace rbd {
+
+
+/*!
+ * This class covers the actuator dynamics for a robot, i.e. not the dynamics of a single actuator, but
+ * the dynamics of the collection of all actuators in the system
+ * The actuators are assumed to form a symplectic system.
+ */
+template <size_t NJOINTS, size_t ACT_STATE_DIMS = 2 * NJOINTS, typename SCALAR = double>
+class ActuatorDynamicsSymplectic
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ static const size_t ACT_STATE_DIM = ACT_STATE_DIMS;
+ static const size_t ACT_POS_DIM = ACT_STATE_DIMS / 2;
+ static const size_t ACT_VEL_DIM = ACT_STATE_DIMS / 2;
+
+ typedef ct::core::StateVector<ACT_STATE_DIMS, SCALAR> act_state_vector_t;
+ typedef ct::core::StateVector<ACT_POS_DIM, SCALAR> act_pos_vector_t;
+ typedef ct::core::StateVector<ACT_VEL_DIM, SCALAR> act_vel_vector_t;
+
+
+ ActuatorDynamicsSymplectic(){};
+
+ virtual ~ActuatorDynamicsSymplectic(){};
+
+ virtual ActuatorDynamicsSymplectic<NJOINTS, ACT_STATE_DIMS, SCALAR>* clone() const override = 0;
+
+ virtual void computePdot(const ct::rbd::tpl::JointState<NJOINTS, SCALAR>& robotJointState,
+ const act_state_vector_t& x,
+ const act_vel_vector_t& v,
+ const core::ControlVector<NJOINTS, SCALAR>& control,
+ act_pos_vector_t& pDot) = 0;
+
+
+ virtual void computeVdot(const ct::rbd::tpl::JointState<NJOINTS, SCALAR>& robotJointState,
+ const act_state_vector_t& x,
+ const act_pos_vector_t& p,
+ const core::ControlVector<NJOINTS, SCALAR>& control,
+ act_vel_vector_t& vDot) = 0;
+
+
+ //! output equation of the actuator
+ /**
+ * Algebraic output equation for the actuator system, translating the current actuator state and the current robot state into
+ * an output control vector.
+ * Example: in an RBD setting, the controlOutput may be the actual joint torques.
+ * @param robotState current RBD state of the robot
+ * @param actState current state of the actuators
+ * @param controlOutput control output (output side of actuator)
+ */
+ virtual core::ControlVector<NJOINTS, SCALAR> computeControlOutput(
+ const ct::rbd::tpl::JointState<NJOINTS, SCALAR>& robotJointState,
+ const act_state_vector_t& actState) = 0;
+
+ /*!
+ * @brief reconstruct actuator state from a desired control output and robot joint state (e.g. for initialization)
+ * @return the actuator state resulting in the desired control output
+ */
+ virtual ct::core::StateVector<ACT_STATE_DIMS, SCALAR> computeStateFromOutput(
+ const ct::rbd::tpl::JointState<NJOINTS, SCALAR>& refRobotJointState,
+ const core::ControlVector<NJOINTS, SCALAR>& refControl) = 0;
+};
+
+
+} // namespace rbd
+} // namespace ct
diff --git a/ct_rbd/include/ct/rbd/robot/actuator/SEADynamicsFirstOrder-impl.h b/ct_rbd/include/ct/rbd/robot/actuator/SEADynamicsFirstOrder-impl.h
new file mode 100644
index 0000000..a694202
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/actuator/SEADynamicsFirstOrder-impl.h
@@ -0,0 +1,56 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace rbd {
+
+template <size_t NJOINTS, typename SCALAR>
+SEADynamicsFirstOrder<NJOINTS, SCALAR>::SEADynamicsFirstOrder(double k_spring) : k_((SCALAR)k_spring)
+{
+}
+
+template <size_t NJOINTS, typename SCALAR>
+SEADynamicsFirstOrder<NJOINTS, SCALAR>::~SEADynamicsFirstOrder()
+{
+}
+
+template <size_t NJOINTS, typename SCALAR>
+SEADynamicsFirstOrder<NJOINTS, SCALAR>* SEADynamicsFirstOrder<NJOINTS, SCALAR>::clone() const
+{
+ return new SEADynamicsFirstOrder(*this);
+}
+
+template <size_t NJOINTS, typename SCALAR>
+void SEADynamicsFirstOrder<NJOINTS, SCALAR>::computeActuatorDynamics(
+ const ct::rbd::tpl::JointState<NJOINTS, SCALAR>& robotJointState,
+ const ct::core::StateVector<NJOINTS, SCALAR>& state,
+ const SCALAR& t,
+ const ct::core::ControlVector<NJOINTS, SCALAR>& control,
+ ct::core::StateVector<NJOINTS, SCALAR>& derivative)
+{
+ derivative.template head<NJOINTS>() = control.template head<NJOINTS>();
+}
+
+template <size_t NJOINTS, typename SCALAR>
+core::ControlVector<NJOINTS, SCALAR> SEADynamicsFirstOrder<NJOINTS, SCALAR>::computeControlOutput(
+ const ct::rbd::tpl::JointState<NJOINTS, SCALAR>& robotJointState,
+ const typename BASE::act_state_vector_t& actState)
+{
+ return (actState.template topRows<NJOINTS>() - robotJointState.getPositions()) * k_;
+}
+
+template <size_t NJOINTS, typename SCALAR>
+ct::core::StateVector<NJOINTS, SCALAR> SEADynamicsFirstOrder<NJOINTS, SCALAR>::computeStateFromOutput(
+ const ct::rbd::tpl::JointState<NJOINTS, SCALAR>& refRobotJointState,
+ const core::ControlVector<NJOINTS, SCALAR>& refControl)
+{
+ return refRobotJointState.getPositions() + 1 / k_ * refControl.toImplementation();
+}
+
+} // namespace rbd
+} // namespace ct
diff --git a/ct_rbd/include/ct/rbd/robot/actuator/SEADynamicsFirstOrder.h b/ct_rbd/include/ct/rbd/robot/actuator/SEADynamicsFirstOrder.h
new file mode 100644
index 0000000..d4eb6db
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/actuator/SEADynamicsFirstOrder.h
@@ -0,0 +1,54 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "ActuatorDynamics.h"
+
+namespace ct {
+namespace rbd {
+
+/*!
+ * Series-elastic actuator dynamics modelled as a spring, control input is the motor velocity
+ */
+template <size_t NJOINTS, typename SCALAR = double>
+class SEADynamicsFirstOrder : public ActuatorDynamics<NJOINTS, NJOINTS, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef ActuatorDynamics<NJOINTS, NJOINTS, SCALAR> BASE;
+
+ //! constructor assuming unit amplification
+ SEADynamicsFirstOrder(double k_spring);
+
+ //! destructor
+ virtual ~SEADynamicsFirstOrder();
+
+ //! deep cloning
+ virtual SEADynamicsFirstOrder<NJOINTS, SCALAR>* clone() const override;
+
+ virtual void computeActuatorDynamics(const ct::rbd::tpl::JointState<NJOINTS, SCALAR>& robotJointState,
+ const ct::core::StateVector<NJOINTS, SCALAR>& state,
+ const SCALAR& t,
+ const ct::core::ControlVector<NJOINTS, SCALAR>& control,
+ ct::core::StateVector<NJOINTS, SCALAR>& derivative) override;
+
+ virtual core::ControlVector<NJOINTS, SCALAR> computeControlOutput(
+ const ct::rbd::tpl::JointState<NJOINTS, SCALAR>& robotJointState,
+ const typename BASE::act_state_vector_t& actState) override;
+
+ virtual ct::core::StateVector<NJOINTS, SCALAR> computeStateFromOutput(
+ const ct::rbd::tpl::JointState<NJOINTS, SCALAR>& refRobotJointState,
+ const core::ControlVector<NJOINTS, SCALAR>& refControl) override;
+
+private:
+ SCALAR k_; //! spring constant
+};
+
+
+} // namespace rbd
+} // namespace ct
diff --git a/ct_rbd/include/ct/rbd/robot/actuator/SecondOrderActuatorDynamics-impl.h b/ct_rbd/include/ct/rbd/robot/actuator/SecondOrderActuatorDynamics-impl.h
new file mode 100644
index 0000000..dd6d2ae
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/actuator/SecondOrderActuatorDynamics-impl.h
@@ -0,0 +1,107 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace rbd {
+
+template <size_t NJOINTS, typename SCALAR>
+SecondOrderActuatorDynamics<NJOINTS, SCALAR>::SecondOrderActuatorDynamics(double w_n, double zeta)
+ : oscillators_(NJOINTS, ct::core::tpl::SecondOrderSystem<SCALAR>((SCALAR)w_n, (SCALAR)zeta, (SCALAR)(w_n * w_n)))
+{
+}
+
+template <size_t NJOINTS, typename SCALAR>
+SecondOrderActuatorDynamics<NJOINTS, SCALAR>::SecondOrderActuatorDynamics(double w_n, double zeta, double g_dc)
+ : oscillators_(NJOINTS, ct::core::tpl::SecondOrderSystem<SCALAR>((SCALAR)w_n, (SCALAR)zeta, (SCALAR)g_dc))
+{
+}
+
+template <size_t NJOINTS, typename SCALAR>
+SecondOrderActuatorDynamics<NJOINTS, SCALAR>::SecondOrderActuatorDynamics(std::vector<double> w_n,
+ std::vector<double> zeta)
+{
+ if ((w_n.size() != NJOINTS) || (zeta.size() != NJOINTS))
+ throw std::runtime_error("SecondOrderActuatorDynamics: w_n or zeta is not correct size");
+
+ oscillators_.resize(NJOINTS);
+ for (size_t i = 0; i < NJOINTS; i++)
+ oscillators_[i] =
+ ct::core::tpl::SecondOrderSystem<SCALAR>((SCALAR)w_n[i], (SCALAR)zeta[i], (SCALAR)(w_n[i] * w_n[i]));
+}
+
+template <size_t NJOINTS, typename SCALAR>
+SecondOrderActuatorDynamics<NJOINTS, SCALAR>::SecondOrderActuatorDynamics(std::vector<double> w_n,
+ std::vector<double> zeta,
+ std::vector<double> g_dc)
+{
+ if ((w_n.size() != NJOINTS) || (zeta.size() != NJOINTS) || (g_dc.size() != NJOINTS))
+ throw std::runtime_error("SecondOrderActuatorDynamics: w_n, zeta or g_dc is not correct size");
+
+ oscillators_.resize(NJOINTS);
+ for (size_t i = 0; i < NJOINTS; i++)
+ oscillators_[i] = ct::core::tpl::SecondOrderSystem<SCALAR>((SCALAR)w_n[i], (SCALAR)zeta[i], (SCALAR)g_dc[i]);
+}
+
+template <size_t NJOINTS, typename SCALAR>
+SecondOrderActuatorDynamics<NJOINTS, SCALAR>::~SecondOrderActuatorDynamics()
+{
+}
+
+template <size_t NJOINTS, typename SCALAR>
+SecondOrderActuatorDynamics<NJOINTS, SCALAR>* SecondOrderActuatorDynamics<NJOINTS, SCALAR>::clone() const
+{
+ return new SecondOrderActuatorDynamics(*this);
+}
+
+template <size_t NJOINTS, typename SCALAR>
+void SecondOrderActuatorDynamics<NJOINTS, SCALAR>::computeActuatorDynamics(
+ const ct::rbd::tpl::JointState<NJOINTS, SCALAR>& robotJointState,
+ const ct::core::StateVector<2 * NJOINTS, SCALAR>& state,
+ const SCALAR& t,
+ const ct::core::ControlVector<NJOINTS, SCALAR>& control,
+ ct::core::StateVector<2 * NJOINTS, SCALAR>& derivative)
+{
+ // evaluate oscillator dynamics for each joint
+ for (size_t i = 0; i < NJOINTS; i++)
+ {
+ core::StateVector<2, SCALAR> secondOrderState;
+ core::StateVector<2, SCALAR> secondOrderStateDerivative;
+ core::ControlVector<1, SCALAR> inputCtrl;
+ inputCtrl(0) = control(i);
+
+ secondOrderState << state(i), state(i + NJOINTS);
+
+ oscillators_[i].computeControlledDynamics(secondOrderState, SCALAR(0.0), inputCtrl, secondOrderStateDerivative);
+
+ derivative(i) = state(i + NJOINTS);
+ derivative(i + NJOINTS) = secondOrderStateDerivative(1);
+ }
+}
+
+template <size_t NJOINTS, typename SCALAR>
+core::ControlVector<NJOINTS, SCALAR> SecondOrderActuatorDynamics<NJOINTS, SCALAR>::computeControlOutput(
+ const ct::rbd::tpl::JointState<NJOINTS, SCALAR>& robotJointState,
+ const typename BASE::act_state_vector_t& actState)
+{
+ // for this simple actuator dynamics model, the controlOutput is just the "position" coordinates of the actuator state
+ return actState.template topRows<NJOINTS>();
+}
+
+template <size_t NJOINTS, typename SCALAR>
+ct::core::StateVector<2 * NJOINTS, SCALAR> SecondOrderActuatorDynamics<NJOINTS, SCALAR>::computeStateFromOutput(
+ const ct::rbd::tpl::JointState<NJOINTS, SCALAR>& refRobotJointState,
+ const core::ControlVector<NJOINTS, SCALAR>& refControl)
+{
+ ct::core::StateVector<2 * NJOINTS, SCALAR> refState;
+ refState.setZero();
+ refState.template topRows<NJOINTS>() = refControl.toImplementation();
+ return refState;
+}
+
+} // namespace rbd
+} // namespace ct
diff --git a/ct_rbd/include/ct/rbd/robot/actuator/SecondOrderActuatorDynamics.h b/ct_rbd/include/ct/rbd/robot/actuator/SecondOrderActuatorDynamics.h
new file mode 100644
index 0000000..7ed2db5
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/actuator/SecondOrderActuatorDynamics.h
@@ -0,0 +1,64 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "ActuatorDynamics.h"
+
+namespace ct {
+namespace rbd {
+
+/*!
+ * Actuator Dynamics modeled as second order system, an oscillator with damping.
+ *
+ * \warning This is wrong - actually the simple oscillator is not a symplectic system (if damping != 0)
+ */
+template <size_t NJOINTS, typename SCALAR = double>
+class SecondOrderActuatorDynamics : public ActuatorDynamics<2 * NJOINTS, NJOINTS, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef ActuatorDynamics<2 * NJOINTS, NJOINTS, SCALAR> BASE;
+
+ //! constructor assuming unit amplification
+ SecondOrderActuatorDynamics(double w_n, double zeta);
+
+ //! constructor assuming custom amplification, set g_dc = w_n*w_n
+ SecondOrderActuatorDynamics(double w_n, double zeta, double g_dc);
+
+ //! constructor assuming unit amplification
+ SecondOrderActuatorDynamics(std::vector<double> w_n, std::vector<double> zeta);
+
+ //! constructor assuming custom amplification, set g_dc = w_n*w_n
+ SecondOrderActuatorDynamics(std::vector<double> w_n, std::vector<double> zeta, std::vector<double> g_dc);
+
+ //! destructor
+ virtual ~SecondOrderActuatorDynamics();
+
+ //! deep cloning
+ virtual SecondOrderActuatorDynamics<NJOINTS, SCALAR>* clone() const override;
+
+ virtual void computeActuatorDynamics(const ct::rbd::tpl::JointState<NJOINTS, SCALAR>& robotJointState,
+ const ct::core::StateVector<2 * NJOINTS, SCALAR>& state,
+ const SCALAR& t,
+ const ct::core::ControlVector<NJOINTS, SCALAR>& control,
+ ct::core::StateVector<2 * NJOINTS, SCALAR>& derivative) override;
+
+ virtual core::ControlVector<NJOINTS, SCALAR> computeControlOutput(
+ const ct::rbd::tpl::JointState<NJOINTS, SCALAR>& robotJointState,
+ const typename BASE::act_state_vector_t& actState) override;
+
+ virtual ct::core::StateVector<2 * NJOINTS, SCALAR> computeStateFromOutput(
+ const ct::rbd::tpl::JointState<NJOINTS, SCALAR>& refRobotJointState,
+ const core::ControlVector<NJOINTS, SCALAR>& refControl) override;
+
+private:
+ std::vector<ct::core::tpl::SecondOrderSystem<SCALAR>> oscillators_;
+};
+
+} // namespace rbd
+} // namespace ct
diff --git a/ct_rbd/include/ct/rbd/robot/control/IDControllerFB.h b/ct_rbd/include/ct/rbd/robot/control/IDControllerFB.h
new file mode 100644
index 0000000..d36484e
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/control/IDControllerFB.h
@@ -0,0 +1,93 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace rbd {
+
+
+template <class Dynamics>
+class IDControllerFB
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef typename Dynamics::RBDState_t RBDState;
+
+ IDControllerFB(std::shared_ptr<Dynamics> dynamics = std::shared_ptr<Dynamics>(new Dynamics()),
+ const RBDState& desState = RBDState(),
+ const Eigen::Matrix<double, 6, 1>& Kp = Eigen::Matrix<double, 6, 1>::Zero(),
+ const Eigen::Matrix<double, 6, 1>& Kd = Eigen::Matrix<double, 6, 1>::Zero())
+ : dynamics_(dynamics), desState_(desState), Kp_(Kp), Kd_(Kd)
+ {
+ }
+
+ void setDesiredState(const RBDState& desState) { desState_ = desState; }
+ void setPoseGains(const Eigen::Matrix<double, 6, 1>& Kp) { Kp_ = Kp; }
+ void setTwistGains(const Eigen::Matrix<double, 6, 1>& Kd) { Kd_ = Kd; }
+ typename Dynamics::RBDAcceleration_t computeDesiredAcceleration(const RBDState& currBaseState)
+ {
+ // compute rotation error
+ kindr::RotationQuaternionD Bcurr_q_Bdes(
+ currBaseState.basePose().getRotationQuaternion().inverted() * desState_.basePose().getRotationQuaternion());
+ kindr::EulerAnglesXyzPD Bcurr_eul_Bdes(Bcurr_q_Bdes);
+
+ // desired base acceleration
+ Eigen::Matrix<double, 6, 1> aDesEigen = Eigen::Matrix<double, 6, 1>::Zero();
+
+ // base orientation error
+ aDesEigen.segment(0, 3) = Kp_.segment(0, 3).cwiseProduct(Bcurr_eul_Bdes.getUnique().toImplementation());
+
+ // base position error
+ Eigen::Vector3d W_positionError =
+ (desState_.basePose().position() - currBaseState.basePose().position()).toImplementation();
+ Eigen::Vector3d W_weightedPosError = Kp_.segment(3, 3).cwiseProduct(W_positionError);
+ aDesEigen.segment(3, 3) = currBaseState.basePose().rotateInertiaToBase(W_weightedPosError);
+
+ // base velocity error
+ aDesEigen +=
+ Kd_.cwiseProduct(desState_.base().velocities().getVector() - currBaseState.base().velocities().getVector());
+
+ typename Dynamics::RBDAcceleration_t aDes;
+ aDes.setZero();
+
+ aDes.base().fromVector6d(aDesEigen);
+
+ return aDes;
+ }
+
+ typename Dynamics::control_vector_t computeTorque(const RBDState& currState,
+ const typename Dynamics::EE_in_contact_t& eeInContact,
+ bool gravityCompensation = true)
+ {
+ typename Dynamics::RBDAcceleration_t aDes = computeDesiredAcceleration(currState);
+
+ typename Dynamics::control_vector_t uId;
+
+ if (gravityCompensation)
+ {
+ dynamics_->ProjectedInverseDynamics(eeInContact, currState, aDes, uId);
+ }
+ else
+ {
+ dynamics_->ProjectedInverseDynamicsNoGravity(eeInContact, currState, aDes, uId);
+ }
+
+ return uId;
+ }
+
+
+private:
+ std::shared_ptr<Dynamics> dynamics_;
+
+ RBDState desState_;
+
+ Eigen::Matrix<double, 6, 1> Kp_;
+ Eigen::Matrix<double, 6, 1> Kd_;
+};
+}
+}
diff --git a/ct_rbd/include/ct/rbd/robot/control/InfiniteHorizonLQRwithInverseDynamics.h b/ct_rbd/include/ct/rbd/robot/control/InfiniteHorizonLQRwithInverseDynamics.h
new file mode 100644
index 0000000..6af1b70
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/control/InfiniteHorizonLQRwithInverseDynamics.h
@@ -0,0 +1,135 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/rbd/systems/FixBaseFDSystem.h>
+
+namespace ct {
+namespace rbd {
+
+template <class RBDDynamics>
+class InfiniteHorizonLQRwithInverseDynamics : public ct::core::Controller<RBDDynamics::NSTATE, RBDDynamics::NJOINTS>
+{
+public:
+ typedef typename RBDDynamics::SCALAR SCALAR;
+ typedef typename RBDDynamics::control_vector_t control_vector_t;
+ typedef typename RBDDynamics::state_vector_t state_vector_t;
+ typedef typename RBDDynamics::JointAcceleration_t JointAcceleration_t;
+
+ typedef core::StateMatrix<RBDDynamics::NSTATE, SCALAR> state_matrix_t;
+ typedef core::StateMatrix<RBDDynamics::NJOINTS, SCALAR> control_matrix_t;
+ typedef core::StateControlMatrix<RBDDynamics::NSTATE, RBDDynamics::NJOINTS, SCALAR> state_control_matrix_t;
+ typedef core::FeedbackMatrix<RBDDynamics::NSTATE, RBDDynamics::NJOINTS, SCALAR> feedback_matrix_t;
+
+
+ InfiniteHorizonLQRwithInverseDynamics() {}
+ InfiniteHorizonLQRwithInverseDynamics(std::shared_ptr<ct::rbd::FixBaseFDSystem<RBDDynamics>> system,
+ std::shared_ptr<ct::core::LinearSystem<2 * (RBDDynamics::NJOINTS), RBDDynamics::NJOINTS>> linearSystem)
+ : fdSystem_(system),
+ linearSystem_(linearSystem),
+ infiniteHorizonLQR_(),
+ stateSetpoint_(state_vector_t::Zero()),
+ id_torques_(control_vector_t::Zero()),
+ K_(feedback_matrix_t::Zero())
+ {
+ }
+
+ InfiniteHorizonLQRwithInverseDynamics(const InfiniteHorizonLQRwithInverseDynamics& arg)
+ : fdSystem_(arg.fdSystem_->clone()),
+ linearSystem_(arg.linearSystem_->clone()),
+ infiniteHorizonLQR_(),
+ stateSetpoint_(arg.stateSetpoint_),
+ id_torques_(arg.id_torques_),
+ K_(arg.K_)
+ {
+ }
+
+
+ virtual ~InfiniteHorizonLQRwithInverseDynamics() {}
+ virtual InfiniteHorizonLQRwithInverseDynamics* clone() const override
+ {
+ return new InfiniteHorizonLQRwithInverseDynamics<RBDDynamics>(*this);
+ }
+
+
+ virtual void computeControl(const ct::core::StateVector<RBDDynamics::NSTATE, SCALAR>& state,
+ const ct::core::Time& t,
+ ct::core::ControlVector<RBDDynamics::NJOINTS, SCALAR>& controlAction) override
+ {
+ controlAction = id_torques_ - K_ * (state - stateSetpoint_);
+ }
+
+
+ const feedback_matrix_t& getK() { return K_; }
+ bool designInfiniteHorizonLQR(const state_vector_t& stateSetpoint,
+ const state_matrix_t& Q_stab,
+ const control_matrix_t& R_stab,
+ feedback_matrix_t& K)
+ {
+ computeIDTorques(id_torques_, stateSetpoint);
+
+ // local linearizations
+ state_matrix_t A_setpoint = linearSystem_->getDerivativeState(stateSetpoint, id_torques_);
+ state_control_matrix_t B_setpoint = linearSystem_->getDerivativeControl(stateSetpoint, id_torques_);
+
+ bool success = infiniteHorizonLQR_.compute(Q_stab, R_stab, A_setpoint, B_setpoint, K, false, true);
+
+ // save to local class members
+ stateSetpoint_ = stateSetpoint;
+ K_ = K;
+
+ return success;
+ }
+
+
+ /*
+ * call this design method if you don't want to get the feedback gain matrix explicitly
+ */
+ bool designInfiniteHorizonLQR(const state_vector_t& stateSetpoint,
+ const state_matrix_t& Q_stab,
+ const control_matrix_t& R_stab)
+ {
+ feedback_matrix_t K_temp = feedback_matrix_t::Zero();
+
+ return designInfiniteHorizonLQR(stateSetpoint, Q_stab, R_stab, K_temp);
+ }
+
+
+ const control_vector_t& getIDTorques() const { return id_torques_; }
+ const state_vector_t& getStateSetpoint() const { return stateSetpoint_; }
+ void setNonlinearSystem(std::shared_ptr<ct::rbd::FixBaseFDSystem<RBDDynamics>> system) { fdSystem_ = system; }
+ void setLinearSystem(
+ std::shared_ptr<ct::core::LinearSystem<2 * (RBDDynamics::NJOINTS), RBDDynamics::NJOINTS>> system)
+ {
+ linearSystem_ = system;
+ }
+
+private:
+ /*
+ * compute desired joint torques through inverse dynamics
+ */
+ void computeIDTorques(control_vector_t& id_torques,
+ const state_vector_t& stateSetpoint,
+ const JointAcceleration_t& qdd = JointAcceleration_t::Zero())
+ {
+ typename RBDDynamics::ExtLinkForces_t extlinkforce(Eigen::Matrix<SCALAR, 6, 1>::Zero());
+
+ fdSystem_->dynamics().FixBaseID(stateSetpoint, qdd, extlinkforce, id_torques);
+ }
+
+
+ std::shared_ptr<ct::rbd::FixBaseFDSystem<RBDDynamics>> fdSystem_;
+ std::shared_ptr<ct::core::LinearSystem<2 * (RBDDynamics::NJOINTS), RBDDynamics::NJOINTS>> linearSystem_;
+
+ ct::optcon::LQR<RBDDynamics::NSTATE, RBDDynamics::NJOINTS> infiniteHorizonLQR_;
+
+ state_vector_t stateSetpoint_;
+ control_vector_t id_torques_;
+ feedback_matrix_t K_;
+};
+}
+}
diff --git a/ct_rbd/include/ct/rbd/robot/control/JointPositionPIDController-impl.h b/ct_rbd/include/ct/rbd/robot/control/JointPositionPIDController-impl.h
new file mode 100644
index 0000000..169748d
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/control/JointPositionPIDController-impl.h
@@ -0,0 +1,104 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace rbd {
+
+template <size_t NJOINTS>
+JointPositionPIDController<NJOINTS>* JointPositionPIDController<NJOINTS>::clone() const
+{
+ throw std::runtime_error("RBD: JointPositionPIDController.h, clone() not implemented");
+};
+
+template <size_t NJOINTS>
+JointPositionPIDController<NJOINTS>::~JointPositionPIDController()
+{
+}
+
+template <size_t NJOINTS>
+JointPositionPIDController<NJOINTS>::JointPositionPIDController(const Eigen::Matrix<double, NJOINTS, 1>& desiredPosition,
+ const Eigen::Matrix<double, NJOINTS, 1>& desiredVelocity,
+ const std::vector<PIDController::parameters_t>& parameters)
+{
+ assert(parameters.size() == NJOINTS);
+
+ for (size_t i = 0; i < NJOINTS; i++)
+ {
+ jointControllers_.push_back(PIDController(
+ parameters[i], PIDController::setpoint_t(desiredPosition(i), desiredVelocity(i))));
+ }
+}
+
+template <size_t NJOINTS>
+JointPositionPIDController<NJOINTS>::JointPositionPIDController(const Eigen::Matrix<double, NJOINTS, 1>& desiredPosition,
+ const Eigen::Matrix<double, NJOINTS, 1>& desiredVelocity,
+ const PIDController::parameters_t& parameters)
+{
+ for (size_t i = 0; i < NJOINTS; i++)
+ {
+ jointControllers_.push_back(
+ PIDController(parameters, PIDController::setpoint_t(desiredPosition(i), desiredVelocity(i))));
+ }
+}
+
+template <size_t NJOINTS>
+void JointPositionPIDController<NJOINTS>::computeControl(const core::StateVector<STATE_DIM>& state,
+ const core::Time& t,
+ core::ControlVector<NJOINTS>& control)
+{
+ ct::rbd::JointState<NJOINTS> jstate(state);
+
+ for (size_t i = 0; i < NJOINTS; i++)
+ {
+ control(i) = jointControllers_[i].computeControl(jstate.getPosition(i), jstate.getVelocity(i), t);
+ }
+}
+
+template <size_t NJOINTS>
+void JointPositionPIDController<NJOINTS>::setDesiredPosition(const Eigen::Matrix<double, NJOINTS, 1>& desiredPosition)
+{
+ for (size_t i = 0; i < NJOINTS; i++)
+ {
+ jointControllers_[i].setDesiredState(desiredPosition(i));
+ };
+}
+
+template <size_t NJOINTS>
+void JointPositionPIDController<NJOINTS>::setDesiredPosition(double desiredPosition, int jointId)
+{
+ assert(0 <= jointId && jointId < NJOINTS); // assuming first joint has index 0
+
+ jointControllers_[jointId].setDesiredState(desiredPosition);
+}
+
+template <size_t NJOINTS>
+void JointPositionPIDController<NJOINTS>::setAllPIDGains(double kp, double ki, double kd)
+{
+ PIDController::parameters_t parameters;
+ parameters.k_p = kp;
+ parameters.k_i = ki;
+ parameters.k_d = kd;
+
+ for (size_t i = 0; i < NJOINTS; i++)
+ {
+ jointControllers_[i].changeParameters(parameters);
+ }
+}
+
+template <size_t NJOINTS>
+void JointPositionPIDController<NJOINTS>::reset()
+{
+ for (size_t i = 0; i < NJOINTS; i++)
+ {
+ jointControllers_[i].reset();
+ }
+}
+
+
+} // namespace rbd
+} // namespace ct
diff --git a/ct_rbd/include/ct/rbd/robot/control/JointPositionPIDController.h b/ct_rbd/include/ct/rbd/robot/control/JointPositionPIDController.h
new file mode 100644
index 0000000..e52b57b
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/control/JointPositionPIDController.h
@@ -0,0 +1,60 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+#include <vector>
+
+namespace ct {
+namespace rbd {
+
+/**
+ * \brief A joint position controller using a PID controller for all joints
+ *
+ * \tparam NJOINTS number of joints of the robot
+ */
+template <size_t NJOINTS>
+class JointPositionPIDController : public ct::core::Controller<2 * NJOINTS, NJOINTS>
+{
+public:
+ static const size_t STATE_DIM = 2 * NJOINTS;
+ static const size_t CONTROL_DIM = NJOINTS;
+
+ typedef std::shared_ptr<JointPositionPIDController<NJOINTS>> Ptr;
+ typedef ct::core::PIDController<double> PIDController;
+
+ virtual JointPositionPIDController<NJOINTS>* clone() const override;
+
+ JointPositionPIDController(
+ const Eigen::Matrix<double, NJOINTS, 1>& desiredPosition = Eigen::Matrix<double, NJOINTS, 1>::Zero(),
+ const Eigen::Matrix<double, NJOINTS, 1>& desiredVelocity = Eigen::Matrix<double, NJOINTS, 1>::Zero(),
+ const std::vector<PIDController::parameters_t>& parameters = std::vector<PIDController::parameters_t>(NJOINTS,
+ PIDController::parameters_t()));
+
+ JointPositionPIDController(const Eigen::Matrix<double, NJOINTS, 1>& desiredPosition,
+ const Eigen::Matrix<double, NJOINTS, 1>& desiredVelocity,
+ const PIDController::parameters_t& parameters);
+
+ virtual ~JointPositionPIDController();
+
+ void computeControl(const core::StateVector<STATE_DIM>& state,
+ const core::Time& t,
+ core::ControlVector<NJOINTS>& control) override;
+
+ void setDesiredPosition(const Eigen::Matrix<double, NJOINTS, 1>& desiredPosition);
+
+ void setDesiredPosition(double desiredPosition, int jointId);
+
+ void setAllPIDGains(double kp, double ki, double kd);
+
+ void reset();
+
+protected:
+ std::vector<PIDController> jointControllers_;
+};
+
+} // namespace rbd
+} // namespace ct
diff --git a/ct_rbd/include/ct/rbd/robot/control/SelectionMatrix-impl.h b/ct_rbd/include/ct/rbd/robot/control/SelectionMatrix-impl.h
new file mode 100644
index 0000000..cacba4d
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/control/SelectionMatrix-impl.h
@@ -0,0 +1,54 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace rbd {
+
+template <size_t CONTROL_DIM, size_t STATE_DIM, typename SCALAR>
+SelectionMatrix<CONTROL_DIM, STATE_DIM, SCALAR>::SelectionMatrix(const SelectionMatrix& other)
+ : Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM>(other)
+{
+}
+
+template <size_t CONTROL_DIM, size_t STATE_DIM, typename SCALAR>
+SelectionMatrix<CONTROL_DIM, STATE_DIM, SCALAR>::SelectionMatrix(bool floatingBase)
+ : Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM>()
+{
+ setIdentity(floatingBase);
+}
+
+template <size_t CONTROL_DIM, size_t STATE_DIM, typename SCALAR>
+template <typename T>
+typename std::enable_if<(STATE_DIM >= 6), T>::type SelectionMatrix<CONTROL_DIM, STATE_DIM, SCALAR>::setIdentity(
+ bool floatingBase)
+{
+ this->setZero();
+
+ if (floatingBase)
+ {
+ this->template bottomRightCorner<CONTROL_DIM, STATE_DIM - 6>().setIdentity();
+ }
+ else
+ {
+ this->template bottomRightCorner<CONTROL_DIM, CONTROL_DIM>().setIdentity();
+ }
+}
+
+template <size_t CONTROL_DIM, size_t STATE_DIM, typename SCALAR>
+template <typename T>
+typename std::enable_if<(STATE_DIM < 6), T>::type SelectionMatrix<CONTROL_DIM, STATE_DIM, SCALAR>::setIdentity(
+ bool floatingBase)
+{
+ if (floatingBase)
+ throw std::runtime_error("Selection Matrix for floating base systems should at least have 6 columns");
+
+ this->setZero();
+ this->template bottomRightCorner<CONTROL_DIM, CONTROL_DIM>().setIdentity();
+}
+}
+}
diff --git a/ct_rbd/include/ct/rbd/robot/control/SelectionMatrix.h b/ct_rbd/include/ct/rbd/robot/control/SelectionMatrix.h
new file mode 100644
index 0000000..2351574
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/control/SelectionMatrix.h
@@ -0,0 +1,45 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace rbd {
+
+/**
+ * \brief Selection Matrix for a Rigid Body Dynamics System
+ *
+ * This matrix maps control inputs to the states used in standard rigid body dynamics formulations such as
+ *
+ * \f[
+ * M \ddot{q} + C + G = S^T \tau + J_c \lambda
+ * \f]
+ *
+ * where \f$ S \f$ is the selection matrix.
+ */
+template <size_t CONTROL_DIM, size_t STATE_DIM, typename SCALAR = double>
+class SelectionMatrix : public Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ SelectionMatrix() = delete;
+
+ SelectionMatrix(const SelectionMatrix& other);
+
+ SelectionMatrix(bool floatingBase);
+
+ template <typename T = void> // do not use this template argument
+ typename std::enable_if<(STATE_DIM < 6), T>::type setIdentity(bool floatingBase);
+
+ template <typename T = void> // do not use this template argument
+ typename std::enable_if<(STATE_DIM >= 6), T>::type setIdentity(bool floatingBase);
+
+
+private:
+};
+}
+}
diff --git a/ct_rbd/include/ct/rbd/robot/control/WholeBodyController-impl.h b/ct_rbd/include/ct/rbd/robot/control/WholeBodyController-impl.h
new file mode 100644
index 0000000..320ca7e
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/control/WholeBodyController-impl.h
@@ -0,0 +1,43 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace rbd {
+
+template <size_t NJOINTS>
+WholeBodyController<NJOINTS>::WholeBodyController(){};
+
+template <size_t NJOINTS>
+WholeBodyController<NJOINTS>::~WholeBodyController(){};
+
+template <size_t NJOINTS>
+WholeBodyController<NJOINTS>* WholeBodyController<NJOINTS>::clone() const
+{
+ throw std::runtime_error("Not implemented");
+};
+
+template <size_t NJOINTS>
+void WholeBodyController<NJOINTS>::computeControl(const core::StateVector<STATE_DIM>& state,
+ const core::Time& t,
+ core::ControlVector<NJOINTS>& control)
+{
+ ct::rbd::RBDState<NJOINTS> x;
+ x.fromStateVectorEulerXyz(state);
+ core::StateVector<2 * NJOINTS> jState = x.joints().toImplementation();
+
+ jointController_.computeControl(jState, t, control);
+}
+
+template <size_t NJOINTS>
+JointPositionPIDController<NJOINTS>& WholeBodyController<NJOINTS>::getJointController()
+{
+ return jointController_;
+}
+
+} // namespace rbd
+} // namespace ct
diff --git a/ct_rbd/include/ct/rbd/robot/control/WholeBodyController.h b/ct_rbd/include/ct/rbd/robot/control/WholeBodyController.h
new file mode 100644
index 0000000..c206d48
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/control/WholeBodyController.h
@@ -0,0 +1,41 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "JointPositionPIDController.h"
+
+namespace ct {
+namespace rbd {
+
+/**
+ * @class WholeBodyController
+ */
+template <size_t NJOINTS>
+class WholeBodyController : public ct::core::Controller<2 * 6 + 2 * NJOINTS, NJOINTS>
+{
+public:
+ static const size_t STATE_DIM = 2 * 6 + 2 * NJOINTS;
+
+ WholeBodyController();
+
+ virtual ~WholeBodyController();
+
+ virtual WholeBodyController<NJOINTS>* clone() const override;
+
+ virtual void computeControl(const core::StateVector<STATE_DIM>& state,
+ const core::Time& t,
+ core::ControlVector<NJOINTS>& control) override;
+
+ JointPositionPIDController<NJOINTS>& getJointController();
+
+
+protected:
+ JointPositionPIDController<NJOINTS> jointController_;
+};
+
+} // namespace rbd
+} // namespace ct
diff --git a/ct_rbd/include/ct/rbd/robot/costfunction/TermTaskspacePose.hpp b/ct_rbd/include/ct/rbd/robot/costfunction/TermTaskspacePose.hpp
new file mode 100644
index 0000000..32f87f5
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/costfunction/TermTaskspacePose.hpp
@@ -0,0 +1,259 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/optcon/costfunction/term/TermBase.hpp>
+#include <ct/optcon/costfunction/utility/utilities.hpp>
+
+#include <iit/rbd/traits/TraitSelector.h>
+#include <ct/rbd/robot/Kinematics.h>
+#include <ct/rbd/state/RBDState.h>
+
+
+namespace ct {
+namespace rbd {
+
+
+/*!
+ * \brief A costfunction term that defines a cost on a task-space pose
+ *
+ * @todo add velocity to term
+ *
+ * \tparam KINEMATICS kinematics of the system
+ * \tparam FB true if system is a floating base robot
+ * \tparam STATE_DIM state dimensionality of the system
+ * \tparam CONTROL_DIM control dimensionality of the system
+ *
+ */
+template <class KINEMATICS, bool FB, size_t STATE_DIM, size_t CONTROL_DIM>
+class TermTaskspacePose : public optcon::TermBase<STATE_DIM, CONTROL_DIM, double, ct::core::ADCGScalar>
+{
+public:
+ using SCALAR = ct::core::ADCGScalar;
+ using BASE = optcon::TermBase<STATE_DIM, CONTROL_DIM, double, ct::core::ADCGScalar>;
+
+ using RBDStateTpl = ct::rbd::tpl::RBDState<KINEMATICS::NJOINTS, SCALAR>;
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ //! the trivial constructor is explicitly forbidden
+ TermTaskspacePose() {}
+ //! constructor using a quaternion for orientation
+ TermTaskspacePose(size_t eeInd,
+ const Eigen::Matrix<double, 3, 3>& Qpos,
+ const double& Qrot,
+ const core::StateVector<3, double>& w_pos_des,
+ const Eigen::Quaternion<double>& w_q_des,
+ const std::string& name = "TermTaskSpace")
+ : BASE(name),
+ eeInd_(eeInd),
+ Q_pos_(Qpos),
+ Q_rot_(Qrot),
+ w_p_ref_(w_pos_des),
+ w_R_ref_(w_q_des.normalized().toRotationMatrix())
+ {
+ }
+
+ //! constructor using euler angles for orientation
+ TermTaskspacePose(size_t eeInd,
+ const Eigen::Matrix<double, 3, 3>& Qpos,
+ const double& Qrot,
+ const core::StateVector<3, double>& w_pos_des,
+ const Eigen::Matrix<double, 3, 1>& eulerXyz,
+ const std::string& name = "TermTaskSpace")
+ // delegate constructor
+ : TermTaskspacePose(eeInd,
+ Qpos,
+ Qrot,
+ w_pos_des,
+ Eigen::Quaternion<double>(Eigen::AngleAxisd(eulerXyz(0), Eigen::Vector3d::UnitX()) *
+ Eigen::AngleAxisd(eulerXyz(1), Eigen::Vector3d::UnitY()) *
+ Eigen::AngleAxisd(eulerXyz(2), Eigen::Vector3d::UnitZ())),
+ name)
+ {
+ }
+
+
+ //! construct this term with info loaded from a configuration file
+ TermTaskspacePose(std::string& configFile, const std::string& termName, bool verbose = false)
+ {
+ loadConfigFile(configFile, termName, verbose);
+ }
+
+ //! copy constructor
+ TermTaskspacePose(const TermTaskspacePose& arg)
+ : BASE(arg),
+ eeInd_(arg.eeInd_),
+ kinematics_(KINEMATICS()),
+ Q_pos_(arg.Q_pos_),
+ Q_rot_(arg.Q_rot_),
+ w_p_ref_(arg.w_p_ref_),
+ w_R_ref_(arg.w_R_ref_)
+ {
+ }
+
+ //! destructor
+ virtual ~TermTaskspacePose() {}
+ //! deep cloning
+ virtual TermTaskspacePose<KINEMATICS, FB, STATE_DIM, CONTROL_DIM>* clone() const override
+ {
+ return new TermTaskspacePose(*this);
+ }
+
+ //! evaluate
+ virtual SCALAR evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
+ const SCALAR& t) override
+ {
+ return evalLocal<SCALAR>(x, u, t);
+ }
+
+ //! we overload the evaluateCppadCg method
+ virtual ct::core::ADCGScalar evaluateCppadCg(const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t) override
+ {
+ return evalLocal<ct::core::ADCGScalar>(x, u, t);
+ }
+
+ //! load term information from configuration file (stores data in member variables)
+ void loadConfigFile(const std::string& filename, const std::string& termName, bool verbose = false) override
+ {
+ if (verbose)
+ std::cout << "Loading TermTaskspacePose from file " << filename << std::endl;
+
+ ct::optcon::loadScalarCF(filename, "eeId", eeInd_, termName);
+ ct::optcon::loadScalarCF(filename, "Q_rot", Q_rot_, termName);
+
+ ct::optcon::loadMatrixCF(filename, "Q_pos", Q_pos_, termName);
+ ct::optcon::loadMatrixCF(filename, "x_des", w_p_ref_, termName);
+
+ // try loading euler angles
+ if (verbose)
+ std::cout << "trying to load euler angles" << std::endl;
+ try
+ {
+ Eigen::Vector3d eulerXyz;
+ ct::optcon::loadMatrixCF(filename, "eulerXyz_des", eulerXyz, termName);
+ Eigen::Quaternion<double> quat_des(Eigen::AngleAxisd(eulerXyz(0), Eigen::Vector3d::UnitX()) *
+ Eigen::AngleAxisd(eulerXyz(1), Eigen::Vector3d::UnitY()) *
+ Eigen::AngleAxisd(eulerXyz(2), Eigen::Vector3d::UnitZ()));
+ w_R_ref_ = quat_des.toRotationMatrix();
+ if (verbose)
+ std::cout << "Read desired Euler Angles Xyz as = \n" << eulerXyz.transpose() << std::endl;
+ } catch (const std::exception& e)
+ {
+ throw std::runtime_error(
+ "Failed to load TermTaskspacePose, could not find a desired end effector orientation in file.");
+ }
+
+ if (verbose)
+ {
+ std::cout << "Read eeId as eeId = \n" << eeInd_ << std::endl;
+ std::cout << "Read Q_pos as Q_pos = \n" << Q_pos_ << std::endl;
+ std::cout << "Read Q_rot as Q_rot = \n" << Q_rot_ << std::endl;
+ std::cout << "Read x_des as x_des = \n" << w_p_ref_.transpose() << std::endl;
+ }
+ }
+
+
+ //! retrieve reference position in world frame
+ const Eigen::Matrix<double, 3, 1>& getReferencePosition() const { return w_p_ref_; }
+ //! retrieve reference ee orientation in world frame
+ const Eigen::Quaterniond getReferenceOrientation() const { return Eigen::Quaterniond(w_R_ref_); }
+ void setReferencePosition(const Eigen::Matrix<double, 3, 1>& w_p_ref) { w_p_ref_ = w_p_ref; }
+ void setReferenceOrientation(const Eigen::Matrix<double, 3, 3> w_R_ref) { w_R_ref_ = w_R_ref; }
+protected:
+ /*!
+ * setStateFromVector transforms your (custom) state vector x into a RBDState.
+ * Is virtual to allow for easy overloading of this term for custom systems
+ * @param x your state vector
+ * @return a full rigid body state
+ */
+ virtual RBDStateTpl setStateFromVector(const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x)
+ {
+ return setStateFromVector_specialized<FB>(x);
+ }
+
+private:
+ //! computes RBDState in case the user supplied a floating-base robot (SFINAE principle)
+ template <bool T>
+ RBDStateTpl setStateFromVector_specialized(const Eigen::Matrix<SCALAR, -1, 1>& x,
+ typename std::enable_if<T, bool>::type = true)
+ {
+ RBDStateTpl rbdState;
+ rbdState.fromStateVectorEulerXyz(x);
+
+ return rbdState;
+ }
+
+ //! computes RBDState in case the user supplied a fixed-base robot (SFINAE principle)
+ template <bool T>
+ RBDStateTpl setStateFromVector_specialized(const Eigen::Matrix<SCALAR, -1, 1>& x,
+ typename std::enable_if<!T, bool>::type = true)
+ {
+ RBDStateTpl rbdState;
+ rbdState.joints().toImplementation() = x.head(STATE_DIM);
+ return rbdState;
+ }
+
+ //! a templated evaluate() method
+ template <typename SC>
+ SC evalLocal(const Eigen::Matrix<SC, STATE_DIM, 1>& x, const Eigen::Matrix<SC, CONTROL_DIM, 1>& u, const SC& t)
+ {
+ using Matrix3Tpl = Eigen::Matrix<SC, 3, 3>;
+
+ // transform the robot state vector into a CT RBDState
+ tpl::RBDState<KINEMATICS::NJOINTS, SC> rbdState = setStateFromVector(x);
+
+ // position difference in world frame
+ Eigen::Matrix<SC, 3, 1> xDiff =
+ kinematics_.getEEPositionInWorld(eeInd_, rbdState.basePose(), rbdState.jointPositions())
+ .toImplementation() -
+ w_p_ref_.template cast<SC>();
+
+ // compute the cost based on the position error
+ SC pos_cost = (xDiff.transpose() * Q_pos_.template cast<SC>() * xDiff)(0, 0);
+
+
+ // get current end-effector rotation in world frame
+ Matrix3Tpl ee_rot = kinematics_.getEERotInWorld(eeInd_, rbdState.basePose(), rbdState.jointPositions());
+
+ // compute a measure for the difference between current rotation and desired rotation and compute cost based on the orientation error
+ // for the intuition behind, consider the following posts:
+ // https://math.stackexchange.com/a/87698
+ // https://math.stackexchange.com/a/773635
+ Matrix3Tpl ee_rot_diff = w_R_ref_.template cast<SC>().transpose() * ee_rot;
+
+ SC rot_cost =
+ (SC)Q_rot_ * (ee_rot_diff - Matrix3Tpl::Identity()).squaredNorm(); // the frobenius norm of (R_diff-I)
+
+ return pos_cost + rot_cost;
+ }
+
+ //! index of the end-effector in question
+ size_t eeInd_;
+
+ //! the robot kinematics
+ KINEMATICS kinematics_;
+
+ //! weighting matrix for the task-space position
+ Eigen::Matrix<double, 3, 3> Q_pos_;
+
+ //! weighting factor for orientation error
+ double Q_rot_;
+
+ //! reference position in world frame
+ Eigen::Matrix<double, 3, 1> w_p_ref_;
+
+ //! reference ee orientation in world frame
+ Eigen::Matrix<double, 3, 3> w_R_ref_;
+};
+
+
+} // namespace rbd
+} // namespace ct
diff --git a/ct_rbd/include/ct/rbd/robot/costfunction/TermTaskspacePosition.hpp b/ct_rbd/include/ct/rbd/robot/costfunction/TermTaskspacePosition.hpp
new file mode 100644
index 0000000..b1bcd6a
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/costfunction/TermTaskspacePosition.hpp
@@ -0,0 +1,165 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/optcon/costfunction/term/TermBase.hpp>
+#include <ct/optcon/costfunction/utility/utilities.hpp>
+
+#include <iit/rbd/traits/TraitSelector.h>
+#include <ct/rbd/robot/Kinematics.h>
+#include <ct/rbd/state/RBDState.h>
+
+
+namespace ct {
+namespace rbd {
+
+
+/*!
+ * \brief A costfunction term that defines a cost on a taskspace position.
+ * \warning This term does not consider orientations. For a full pose cost function, see TermTaskspacePose.h
+ *
+ * This cost function adds a quadratic penalty on the position offset of an endeffector to a desired position
+ * @todo add velocity to term
+ *
+ * \tparam KINEMATICS kinematics of the system
+ * \tparam FB true if system is a floating base robot
+ * \tparam STATE_DIM state dimensionality of the system
+ * \tparam CONTROL_DIM control dimensionality of the system
+ */
+template <class KINEMATICS, bool FB, size_t STATE_DIM, size_t CONTROL_DIM>
+class TermTaskspacePosition : public optcon::TermBase<STATE_DIM, CONTROL_DIM, double, ct::core::ADCGScalar>
+{
+public:
+ using SCALAR = ct::core::ADCGScalar;
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ //! the trivial constructor is explicitly forbidden
+ TermTaskspacePosition() = delete;
+
+ //! constructor
+ TermTaskspacePosition(size_t eeInd,
+ const Eigen::Matrix<double, 3, 3>& Q,
+ const core::StateVector<3, double>& pos_des,
+ const std::string& name = "TermTaskSpace")
+ : optcon::TermBase<STATE_DIM, CONTROL_DIM, double, ct::core::ADCGScalar>(name),
+ eeInd_(eeInd),
+ QTaskSpace_(Q),
+ pos_ref_(pos_des)
+ {
+ // Checks whether STATE_DIM has the appropriate size.
+ // 2 * (FB * 6 + KINEMATICS::NJOINTS)) represents a floating base system with euler angles
+ // 2 * (FB * 6 + KINEMATICS::NJOINTS) + 1 representing a floating base system with quaternion angles
+ static_assert(
+ (STATE_DIM == 2 * (FB * 6 + KINEMATICS::NJOINTS)) || (STATE_DIM == 2 * (FB * 6 + KINEMATICS::NJOINTS) + 1),
+ "STATE_DIM does not have appropriate size.");
+ }
+
+ //! construct this term with info loaded from a configuration file
+ TermTaskspacePosition(std::string& configFile, const std::string& termName, bool verbose = false)
+ {
+ loadConfigFile(configFile, termName, verbose);
+ }
+
+ //! copy constructor
+ TermTaskspacePosition(const TermTaskspacePosition& arg)
+ : eeInd_(arg.eeInd_), kinematics_(KINEMATICS()), QTaskSpace_(arg.QTaskSpace_), pos_ref_(arg.pos_ref_)
+ {
+ }
+
+ //! destructor
+ virtual ~TermTaskspacePosition() {}
+ //! deep cloning
+ TermTaskspacePosition<KINEMATICS, FB, STATE_DIM, CONTROL_DIM>* clone() const override
+ {
+ return new TermTaskspacePosition(*this);
+ }
+ virtual SCALAR evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
+ const SCALAR& t) override
+ {
+ return evalLocal<SCALAR>(x, u, t);
+ }
+
+ //! we overload the evaluateCppadCg method
+ virtual ct::core::ADCGScalar evaluateCppadCg(const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t) override
+ {
+ return evalLocal<ct::core::ADCGScalar>(x, u, t);
+ }
+
+ //! load term information from configuration file (stores data in member variables)
+ void loadConfigFile(const std::string& filename, const std::string& termName, bool verbose = false) override
+ {
+ ct::optcon::loadScalarCF(filename, "eeId", eeInd_, termName);
+ ct::optcon::loadMatrixCF(filename, "Q", QTaskSpace_, termName);
+ ct::optcon::loadMatrixCF(filename, "x_des", pos_ref_, termName);
+
+ if (verbose)
+ {
+ std::cout << "Read eeId as eeId = \n" << eeInd_ << std::endl;
+ std::cout << "Read Q as Q = \n" << QTaskSpace_ << std::endl;
+ std::cout << "Read x_des as x_des = \n" << pos_ref_.transpose() << std::endl;
+ }
+ }
+
+
+private:
+ //! a templated evaluate() method
+ template <typename SC>
+ SC evalLocal(const Eigen::Matrix<SC, STATE_DIM, 1>& x, const Eigen::Matrix<SC, CONTROL_DIM, 1>& u, const SC& t)
+ {
+ tpl::RBDState<KINEMATICS::NJOINTS, SC> rbdState = setStateFromVector<FB>(x);
+
+ Eigen::Matrix<SC, 3, 1> xDiff =
+ kinematics_.getEEPositionInWorld(eeInd_, rbdState.basePose(), rbdState.jointPositions())
+ .toImplementation() -
+ pos_ref_.template cast<SC>();
+
+ SC cost = (xDiff.transpose() * QTaskSpace_.template cast<SC>() * xDiff)(0, 0);
+ return cost;
+ }
+
+
+ //! computes RBDState in case the user supplied a floating-base robot
+ template <bool T>
+ tpl::RBDState<KINEMATICS::NJOINTS, SCALAR> setStateFromVector(const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ typename std::enable_if<T, bool>::type = true)
+ {
+ tpl::RBDState<KINEMATICS::NJOINTS, SCALAR> rbdState;
+ rbdState.fromStateVectorEulerXyz(x);
+
+ return rbdState;
+ }
+
+ //! computes RBDState in case the user supplied a fixed-base robot
+ template <bool T>
+ tpl::RBDState<KINEMATICS::NJOINTS, SCALAR> setStateFromVector(const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ typename std::enable_if<!T, bool>::type = true)
+ {
+ tpl::RBDState<KINEMATICS::NJOINTS, SCALAR> rbdState;
+ rbdState.joints() = x;
+ return rbdState;
+ }
+
+ //! index of the end-effector in question
+ size_t eeInd_;
+
+ //! the robot kinematics
+ KINEMATICS kinematics_;
+
+ //! weighting matrix for the task-space position
+ Eigen::Matrix<double, 3, 3> QTaskSpace_;
+
+ //! reference position in task-space
+ Eigen::Matrix<double, 3, 1> pos_ref_;
+};
+
+
+} // namespace rbd
+} // namespace ct
diff --git a/ct_rbd/include/ct/rbd/robot/jacobian/ConstraintJacobian.h b/ct_rbd/include/ct/rbd/robot/jacobian/ConstraintJacobian.h
new file mode 100644
index 0000000..f88fe84
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/jacobian/ConstraintJacobian.h
@@ -0,0 +1,106 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/rbd/robot/jacobian/OperationalJacobianBase.h>
+#include <ct/rbd/state/RBDState.h>
+
+#include "FrameJacobian.h"
+
+/**
+ * \ingroup OS
+ */
+namespace ct {
+namespace rbd {
+namespace tpl {
+
+template <typename Kinematics, size_t OUTPUTS, size_t NJOINTS, typename SCALAR>
+class ConstraintJacobian : public OperationalJacobianBase<OUTPUTS, NJOINTS, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef tpl::RBDState<NJOINTS, SCALAR> RBDState_t;
+ typedef typename OperationalJacobianBase<OUTPUTS, NJOINTS, SCALAR>::jacobian_t jacobian_t;
+ typedef Eigen::Matrix<SCALAR, 3, 3> Matrix3s;
+
+ ConstraintJacobian(){};
+
+ virtual ~ConstraintJacobian(){};
+
+ void SetBlockZero(const int& row)
+ {
+ // todo: check if needed
+ this->J();
+ this->J_.template block<3, NJOINTS + 6>(row, 0).setZero();
+ }
+
+ static const size_t BASE_DOF = 6;
+
+ size_t c_size_ = 0;
+ std::vector<int> ee_indices_;
+ std::array<bool, Kinematics::NUM_EE> eeInContact_;
+
+
+ virtual void getJacobianOriginDerivative(const RBDState_t& state, jacobian_t& dJdt)
+ {
+ /*
+ * Takes the numdiff (single sided) of getContactJacobian. derivative is dJ/dt = dJ/dq * dq/dt
+ * Because the contactJacobian (defined in the base frame) is independent of floating base coordinates -> only numdiff against joints
+ */
+ jacobian_t Jc0, Jc1;
+ RBDState_t state1 = state;
+ getJacobianOrigin(state, Jc0);
+
+ SCALAR eps_ = sqrt(Eigen::NumTraits<SCALAR>::epsilon());
+
+ dJdt.setZero();
+ for (size_t i = 0; i < NJOINTS; i++) //
+ {
+ SCALAR h = eps_ * std::max(core::tpl::TraitSelector<SCALAR>::Trait::fabs(state.joints().getPositions()(i)),
+ SCALAR(1.0)); // h = eps_ * max(abs(qj(i)), 1.0)
+ state1.joints().getPositions()(i) += h;
+ getJacobianOrigin(state1, Jc1);
+ dJdt += (Jc1 - Jc0) / h * state.joints().getVelocities()(i);
+ state1.joints().getPositions()(i) = state.joints().getPositions()(i);
+ }
+ }
+
+ virtual void getJacobianOrigin(const RBDState_t& state, jacobian_t& Jc)
+ {
+ Jc.setZero();
+ for (size_t ee = 0; ee < ee_indices_.size(); ee++)
+ {
+ if (eeInContact_[ee_indices_[ee]])
+ {
+ Eigen::Matrix<SCALAR, 3, NJOINTS + 6> J_eeId;
+ kindr::Position<SCALAR, 3> eePosition =
+ kinematics_.getEEPositionInBase(ee_indices_[ee], state.joints().getPositions());
+ Eigen::Matrix<SCALAR, 3, NJOINTS> J_single =
+ kinematics_.robcogen()
+ .getJacobianBaseEEbyId(ee_indices_[ee], state.joints().getPositions())
+ .template bottomRows<3>();
+ FrameJacobian<NJOINTS, SCALAR>::FromBaseJacToInertiaJacTranslation(
+ Matrix3s::Identity(), eePosition.toImplementation(), J_single, J_eeId);
+ Jc.template block<3, NJOINTS + 6>(ee_indices_[ee] * 3, 0) = J_eeId;
+ }
+ }
+ }
+
+
+private:
+ Kinematics kinematics_;
+};
+
+} // namespace tpl
+
+template <typename Kinematics, size_t OUTPUTS, size_t NJOINTS>
+using ConstraintJacobian = tpl::ConstraintJacobian<Kinematics, OUTPUTS, NJOINTS, double>;
+
+
+} /* namespace rbd*/
+} /* namespace ct*/
diff --git a/ct_rbd/include/ct/rbd/robot/jacobian/FrameJacobian.h b/ct_rbd/include/ct/rbd/robot/jacobian/FrameJacobian.h
new file mode 100644
index 0000000..50b7bfd
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/jacobian/FrameJacobian.h
@@ -0,0 +1,193 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/core/types/StateVector.h>
+
+namespace ct {
+namespace rbd {
+namespace tpl {
+
+/**
+ * \ingroup OS
+ * \brief This class provides methods for converting the non-inertia base frame Jacobian matrix to an user
+ * defined inertia frame (called as inertia frame).
+ */
+template <size_t NUM_JOINTS, typename SCALAR>
+class FrameJacobian
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR, 3, 3> Matrix3s;
+ typedef Eigen::Matrix<SCALAR, 3, 1> Vector3s;
+
+ FrameJacobian() {}
+ ~FrameJacobian() {}
+ /**
+ * This method calculates the inertia frame (frame i) Jacobian (rotational and translation part) given the non-inertia base frame full Jacobian (frame b).
+ *
+ * @param[in] i_R_b Rotation matrix from base frame to the desired inertia frame.
+ * @param[in] b_r_point Position of the desired inertia frame origin expressed in the base frame.
+ * @param[in] b_J_point A 6-by-NUM_JOINTS Jacobian matrix (rotational and translation part) expressed in the base frame.
+ * @param[out] i_J_point A 6-by-(6+NUM_JOINTS) Jacobian matrix (rotational and linear translation) expressed in the desired inertia frame.
+ */
+ static void FromBaseJacobianToInertiaJacobian(const Matrix3s& i_R_b,
+ const Vector3s& b_r_point,
+ const Eigen::Matrix<SCALAR, 6, NUM_JOINTS>& b_J_point,
+ Eigen::Matrix<SCALAR, 6, NUM_JOINTS + 6>& i_J_point)
+ {
+ // orientation
+ Eigen::Matrix<SCALAR, 3, NUM_JOINTS + 6> i_J_poitOrientation;
+ FromBaseJacToInertiaJacOrientation(i_R_b, b_r_point, b_J_point.template topRows<3>(), i_J_poitOrientation);
+ // translation
+ Eigen::Matrix<SCALAR, 3, NUM_JOINTS + 6> i_J_poitTranslation;
+ FromBaseJacToInertiaJacTranslation(i_R_b, b_r_point, b_J_point.template bottomRows<3>(), i_J_poitTranslation);
+
+ i_J_point << i_J_poitOrientation, i_J_poitTranslation;
+ }
+
+ /**
+ * This method calculates the inertia frame (frame i) rotational Jacobian given the non-inertia frame rotational Jacobian (frame b).
+ *
+ * @param[in] i_R_b Rotation matrix from base frame to the desired inertia frame.
+ * @param[in] b_r_point Position of the desired inertia frame origin expressed in the base frame.
+ * @param[in] b_J_point A 3-by-NUM_JOINTS rotational Jacobian matrix expressed in the base frame.
+ * @param[out] i_J_point A 3-by-(6+NUM_JOINTS) rotational Jacobian matrix expressed in the desired inertia frame.
+ */
+ static void FromBaseJacToInertiaJacOrientation(const Matrix3s& i_R_b,
+ const Vector3s& b_r_point,
+ const Eigen::Matrix<SCALAR, 3, NUM_JOINTS>& b_J_point,
+ Eigen::Matrix<SCALAR, 3, NUM_JOINTS + 6>& i_J_point)
+ {
+ i_J_point << i_R_b, Matrix3s::Zero(), i_R_b * b_J_point;
+ }
+
+ /**
+ * This method calculates the inertia frame (frame i) translational Jacobian given the non-inertia base frame translation Jacobian (frame b).
+ *
+ * @param[in] i_R_b Rotation matrix from base frame to the desired inertia frame.
+ * @param[in] b_r_point Position of the desired inertia frame origin expressed in the base frame.
+ * @param[in] b_J_point A 3-by-NUM_JOINTS translational Jacobian matrix expressed in the base frame.
+ * @param[out] i_J_point A 3-by-(6+NUM_JOINTS) translational Jacobian matrix expressed in the desired inertia frame.
+ */
+ static void FromBaseJacToInertiaJacTranslation(const Matrix3s& i_R_b,
+ const Vector3s& b_r_point,
+ const Eigen::Matrix<SCALAR, 3, NUM_JOINTS>& b_J_point,
+ Eigen::Matrix<SCALAR, 3, NUM_JOINTS + 6>& i_J_point)
+ {
+ i_J_point << -i_R_b * CrossProductMatrix(b_r_point), i_R_b, i_R_b * b_J_point;
+ }
+
+
+ /**
+ * This method calculates the time derivative of the inertia frame (frame i) Jacobian (rotational and translation part) given the non-inertia base
+ * frame Jacobian (frame b).
+ *
+ * @param[in] generalizedVelocities Generalized coordinate velocities.
+ * @param[in] i_R_b Rotation matrix from base frame to the desired inertia frame.
+ * @param[in] b_r_point Position of the desired inertia frame origin expressed in the base frame.
+ * @param[in] b_J_point A 6-by-NUM_JOINTS Jacobian matrix (rotational and translation part) expressed in the base frame.
+ * @param[in] b_dJdt_point A 6-by-NUM_JOINTS Jacobian matrix time derivative (rotational and translation part) expressed in the base frame.
+ * @param[out] i_dJdt_point A 6-by-(6+NUM_JOINTS) Jacobian matrix time derivative (rotational and translation part) expressed in the desired inertia frame.
+ */
+ static void FromBaseJacobianDevToInertiaJacobianDev(
+ const Eigen::Matrix<SCALAR, NUM_JOINTS + 6, 1>& generalizedVelocities,
+ const Matrix3s& i_R_b,
+ const Vector3s& b_r_point,
+ const Eigen::Matrix<SCALAR, 6, NUM_JOINTS>& b_J_point,
+ const Eigen::Matrix<SCALAR, 6, NUM_JOINTS>& b_dJdt_point,
+ Eigen::Matrix<SCALAR, 6, NUM_JOINTS + 6>& i_dJdt_point)
+ {
+ // orientation
+ Eigen::Matrix<SCALAR, 3, NUM_JOINTS + 6> i_dJdt_pointOrientation;
+ FromBaseJacobianDevToInertiaJacobianDevOrientation(generalizedVelocities, i_R_b, b_r_point,
+ b_J_point.template topRows<3>(), b_dJdt_point.template topRows<3>(), i_dJdt_pointOrientation);
+ // translation
+ Eigen::Matrix<SCALAR, 3, NUM_JOINTS + 6> i_dJdt_pointTranslation;
+ FromBaseJacobianDevToInertiaJacobianDevTranslation(generalizedVelocities, i_R_b, b_r_point,
+ b_J_point.template bottomRows<3>(), b_dJdt_point.template bottomRows<3>(), i_dJdt_pointTranslation);
+
+ i_dJdt_point << i_dJdt_pointOrientation, i_dJdt_pointTranslation;
+ }
+
+ /**
+ * This method calculates the time derivative of the inertia frame (frame i) rotational Jacobian given the non-inertia base
+ * frame rotational Jacobian (frame b).
+ *
+ * @param[in] generalizedVelocities Generalized coordinate velocities.
+ * @param[in] i_R_b Rotation matrix from base frame to the desired inertia frame.
+ * @param[in] b_r_point Position of the desired inertia frame origin expressed in the base frame.
+ * @param[in] b_J_point A 6-by-NUM_JOINTS rotational Jacobian matrix expressed in the base frame.
+ * @param[in] b_dJdt_point A 6-by-NUM_JOINTS rotational Jacobian matrix time derivative expressed in the base frame.
+ * @param[out] i_dJdt_point A 6-by-(6+NUM_JOINTS) rotational Jacobian matrix time derivative expressed in the desired inertia frame.
+ */
+ static void FromBaseJacobianDevToInertiaJacobianDevOrientation(
+ const Eigen::Matrix<SCALAR, NUM_JOINTS + 6, 1>& generalizedVelocities,
+ const Matrix3s& i_R_b,
+ const Vector3s& b_r_point,
+ const Eigen::Matrix<SCALAR, 3, NUM_JOINTS>& b_J_point,
+ const Eigen::Matrix<SCALAR, 3, NUM_JOINTS>& b_dJdt_point,
+ Eigen::Matrix<SCALAR, 3, NUM_JOINTS + 6>& i_dJdt_point)
+ {
+ // rotation matrix time derivatives
+ Matrix3s i_dRdt_b = i_R_b * CrossProductMatrix(generalizedVelocities.template head<3>());
+ // orientation
+ i_dJdt_point << i_dRdt_b, Matrix3s::Zero(), i_dRdt_b * b_J_point + i_R_b * b_dJdt_point;
+ }
+
+ /**
+ * This method calculates the time derivative of the inertia frame (frame i) translational Jacobian given the non-inertia base
+ * frame rotational Jacobian (frame b).
+ *
+ * @param[in] generalizedVelocities Generalized coordinate velocities.
+ * @param[in] i_R_b Rotation matrix from base frame to the desired inertia frame.
+ * @param[in] b_r_point Position of the desired inertia frame origin expressed in the base frame.
+ * @param[in] b_J_point A 6-by-NUM_JOINTS translational Jacobian matrix expressed in the base frame.
+ * @param[in] b_dJdt_point A 6-by-NUM_JOINTS translational Jacobian matrix time derivative expressed in the base frame.
+ * @param[out] i_dJdt_point A 6-by-(6+NUM_JOINTS) translational Jacobian matrix time derivative expressed in the desired inertia frame.
+ */
+ static void FromBaseJacobianDevToInertiaJacobianDevTranslation(
+ const Eigen::Matrix<SCALAR, NUM_JOINTS + 6, 1>& generalizedVelocities,
+ const Matrix3s& i_R_b,
+ const Vector3s& b_r_point,
+ const Eigen::Matrix<SCALAR, 3, NUM_JOINTS>& b_J_point,
+ const Eigen::Matrix<SCALAR, 3, NUM_JOINTS>& b_dJdt_point,
+ Eigen::Matrix<SCALAR, 3, NUM_JOINTS + 6>& i_dJdt_point)
+ {
+ // velocity of the point in the base frame
+ Vector3s b_v_point = b_J_point * generalizedVelocities.template tail<NUM_JOINTS>();
+ // rotation matrix time derivatives
+ Matrix3s i_dRdt_b = i_R_b * CrossProductMatrix(generalizedVelocities.template head<3>());
+ // translation
+ i_dJdt_point << -i_dRdt_b * CrossProductMatrix(b_r_point) - i_R_b * CrossProductMatrix(b_v_point), i_dRdt_b,
+ i_dRdt_b * b_J_point + i_R_b * b_dJdt_point;
+ }
+
+private:
+ /*
+ * calculates the skew matrix for vector cross product
+ */
+ template <typename Derived>
+ static Matrix3s CrossProductMatrix(const Eigen::DenseBase<Derived>& in)
+ {
+ if (in.innerSize() != 3 || in.outerSize() != 1)
+ throw std::runtime_error("Input argument should be a 3-by-1 vector.");
+
+ Matrix3s out;
+ out << SCALAR(0.0), -in(2), +in(1), +in(2), SCALAR(0.0), -in(0), -in(1), +in(0), SCALAR(0.0);
+ return out;
+ }
+};
+
+} // namespace tpl
+
+template <size_t NUM_JOINTS>
+using FrameJacobian = tpl::FrameJacobian<NUM_JOINTS, double>;
+
+} // namespace rbd
+} // namespace ct
diff --git a/ct_rbd/include/ct/rbd/robot/jacobian/JacobianBase.h b/ct_rbd/include/ct/rbd/robot/jacobian/JacobianBase.h
new file mode 100644
index 0000000..ff2ba37
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/jacobian/JacobianBase.h
@@ -0,0 +1,56 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/rbd/state/RBDState.h>
+
+namespace ct {
+namespace rbd {
+namespace tpl {
+
+/**
+ * \ingroup OS
+ * @Brief Interface class for calculating Jacobain and its time derivative in an inertia frame (called as Origin frame).
+ */
+template <size_t NUM_OUTPUTS, size_t NUM_JOINTS, typename SCALAR>
+class JacobianBase
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef RBDState<NUM_JOINTS, SCALAR> state_t;
+ typedef Eigen::Matrix<SCALAR, NUM_OUTPUTS, 6 + NUM_JOINTS> jacobian_t;
+ typedef Eigen::Matrix<SCALAR, 6 + NUM_JOINTS, NUM_OUTPUTS> jacobian_inv_t;
+
+ JacobianBase(){};
+ virtual ~JacobianBase(){};
+
+ /**
+ * This methods calculates the Jacobian of the floating-base Jacobian in the Origin (World or Inertia)
+ * frame.
+ * @param state State of the RBD
+ * @param J floating-base Jacobian in the Origin frame
+ */
+ virtual void getJacobianOrigin(const state_t& state, jacobian_t& J) = 0;
+
+ /**
+ * This methods calculates the time derivative of the Jacobian of the floating-base function in the
+ * Origin (World or Inertia) frame.
+ * @param state State of the RBD
+ * @param dJdt Time derivative of the floating-base Jacobian in the Origin frame
+ */
+ virtual void getJacobianOriginDerivative(const state_t& state, jacobian_t& dJdt) = 0;
+
+private:
+};
+
+} // namespace tpl
+
+template <size_t NUM_OUTPUTS, size_t NUM_JOINTS>
+using JacobianBase = tpl::JacobianBase<NUM_OUTPUTS, NUM_JOINTS, double>;
+
+} // namespace rbd
+} // namespace ct
diff --git a/ct_rbd/include/ct/rbd/robot/jacobian/OperationalJacobianBase.h b/ct_rbd/include/ct/rbd/robot/jacobian/OperationalJacobianBase.h
new file mode 100644
index 0000000..e71790a
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/jacobian/OperationalJacobianBase.h
@@ -0,0 +1,192 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/rbd/robot/jacobian/JacobianBase.h>
+
+namespace ct {
+namespace rbd {
+namespace tpl {
+
+/**
+ * \ingroup OS
+ * @Brief This class provides extra functionalities for performing the operational space Jacobain matrix algebra.
+ * such as calculating the Moore-Penrose pseudo-inverse and Null space projectors.
+ */
+template <size_t NUM_OUTPUTS, size_t NUM_JOINTS, typename SCALAR>
+class OperationalJacobianBase : public JacobianBase<NUM_OUTPUTS, NUM_JOINTS, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef std::shared_ptr<OperationalJacobianBase<NUM_OUTPUTS, NUM_JOINTS, SCALAR>> ptr;
+ typedef typename JacobianBase<NUM_OUTPUTS, NUM_JOINTS, SCALAR>::state_t state_t;
+ typedef typename JacobianBase<NUM_OUTPUTS, NUM_JOINTS, SCALAR>::jacobian_t jacobian_t;
+ typedef typename JacobianBase<NUM_OUTPUTS, NUM_JOINTS, SCALAR>::jacobian_inv_t jacobian_inv_t;
+ typedef Eigen::Matrix<SCALAR, 6 + NUM_JOINTS, 6 + NUM_JOINTS> square_matrix_t;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;
+
+
+ OperationalJacobianBase() { resetUpdatedFlags(); }
+ virtual ~OperationalJacobianBase() {}
+ /**
+ * This function gets the RBD state and the the user defined weighting matrix in order to calculate the
+ * the floating-base Jacobian, its right pseudo-inverse, and null space and their corresponding time
+ * derivatives.
+ * @param state State of the RBD
+ * @param weighting The weighting function for the calculating the pseudo-inverse and null space
+ */
+ void updateState(const state_t& state, const square_matrix_t& weighting = square_matrix_t::Identity())
+ {
+ resetUpdatedFlags();
+ state_ = state;
+ W_ = weighting;
+ }
+
+ //! This method gets the floating-base Jacobian
+ const jacobian_t& J()
+ {
+ if (JUpdated_ == true)
+ return J_;
+
+ JUpdated_ = true;
+ getJacobianOrigin(state_, J_);
+ return J_;
+ }
+
+ //! This method gets the time derivative of the floating-base Jacobian
+ const jacobian_t& dJdt()
+ {
+ if (dJdtUpdated_ == true)
+ return dJdt_;
+
+ dJdtUpdated_ = true;
+ getJacobianOriginDerivative(state_, dJdt_);
+ return dJdt_;
+ }
+
+ //! This method calculates the right inverse using W as the weighting matrix
+ const jacobian_inv_t& JdagerLDLT()
+ {
+ if (JdagerUpdated_ == true)
+ return Jdager_;
+
+ JdagerUpdated_ = true;
+ Jdager_ = W_ * J().transpose() *
+ (J() * W_ * J().transpose()).ldlt().solve(MatrixXs::Identity(NUM_OUTPUTS, NUM_OUTPUTS));
+ return Jdager_;
+ }
+
+ const jacobian_inv_t& JdagerSVD()
+ {
+ if (JdagerUpdated_ == true)
+ return Jdager_;
+
+ JdagerUpdated_ = true;
+ Eigen::JacobiSVD<MatrixXs> svd(J(), Eigen::ComputeThinU | Eigen::ComputeThinV);
+ Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> sing_values(
+ svd.matrixV().cols(), 1); // size of E has same size as columns of V
+ sing_values = (svd.singularValues().array() > 1e-9).select(svd.singularValues().array().inverse(), 0);
+ Jdager_ = svd.matrixV() * sing_values.asDiagonal() * svd.matrixU().transpose();
+ return Jdager_;
+ }
+
+ //! This method calculates the time derivative of right inverse
+ const jacobian_inv_t& dJdagerdt()
+ {
+ // if(dJdagerdtUpdated_==true) return dJdagerdt_;
+
+ dJdagerdtUpdated_ = true;
+ dJdagerdt_ = -JdagerLDLT() * dJdt() * JdagerLDLT();
+ return dJdagerdt_;
+ }
+
+ //! This method calculates the null space projector
+ const square_matrix_t& P()
+ {
+ if (PUpdated_ == true)
+ return P_;
+
+ PUpdated_ = true;
+ P_ = square_matrix_t::Identity() - JdagerLDLT() * J();
+ return P_;
+ }
+
+ //! This method calculates the time derivative of the null space projector
+ const square_matrix_t& dPdt()
+ {
+ if (dPdtUpdated_ == true)
+ return dPdt_;
+
+ dPdtUpdated_ = true;
+ dPdt_ = -JdagerLDLT() * dJdt() * P();
+ return dPdt_;
+ }
+
+ /**
+ * A method for reseting the user defined flags. This function is called is the new state is set
+ * using the update() method
+ */
+ virtual void resetUserUpdatedFlags() {}
+ /**
+ * This methods calculates the Jacobian of the floating-base Jacobian in the Origin (World or Inertia)
+ * frame.
+ * @param state State of the RBD
+ * @param J floating-base Jacobian in the Origin frame
+ */
+ virtual void getJacobianOrigin(const state_t& state, jacobian_t& J) = 0;
+
+ /**
+ * This methods calculates the time derivative of the Jacobian of the floating-base function in the
+ * Origin (World or Inertia) frame.
+ * @param state State of the RBD
+ * @param dJdt Time derivative of the floating-base Jacobian in the Origin frame
+ */
+ virtual void getJacobianOriginDerivative(const state_t& state, jacobian_t& dJdt) = 0;
+
+
+protected:
+ jacobian_t J_;
+
+private:
+ virtual void resetUpdatedFlags()
+ {
+ JUpdated_ = false;
+ dJdtUpdated_ = false;
+ JdagerUpdated_ = false;
+ dJdagerdtUpdated_ = false;
+ PUpdated_ = false;
+ dPdtUpdated_ = false;
+ resetUserUpdatedFlags();
+ }
+
+ /*
+ * variables
+ */
+ bool JUpdated_;
+ bool dJdtUpdated_;
+ bool JdagerUpdated_;
+ bool dJdagerdtUpdated_;
+ bool PUpdated_;
+ bool dPdtUpdated_;
+
+ state_t state_;
+ square_matrix_t W_;
+
+ jacobian_t dJdt_;
+ jacobian_inv_t Jdager_;
+ jacobian_inv_t dJdagerdt_;
+ square_matrix_t P_;
+ square_matrix_t dPdt_;
+};
+
+} // namespace tpl
+
+template <size_t NUM_OUTPUTS, size_t NUM_JOINTS>
+using OperationalJacobianBase = tpl::OperationalJacobianBase<NUM_OUTPUTS, NUM_JOINTS, double>;
+
+} // namespace rbd
+} // namespace ct
diff --git a/ct_rbd/include/ct/rbd/robot/kinematics/EndEffector-impl.h b/ct_rbd/include/ct/rbd/robot/kinematics/EndEffector-impl.h
new file mode 100644
index 0000000..377112d
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/kinematics/EndEffector-impl.h
@@ -0,0 +1,36 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace rbd {
+
+template <size_t NJOINTS, typename SCALAR>
+EndEffector<NJOINTS, SCALAR>::EndEffector() : linkId_(999){};
+
+template <size_t NJOINTS, typename SCALAR>
+EndEffector<NJOINTS, SCALAR>::~EndEffector(){};
+
+template <size_t NJOINTS, typename SCALAR>
+EndEffector<NJOINTS, SCALAR>::EndEffector(const EndEffector& other) : linkId_(other.linkId_)
+{
+}
+
+template <size_t NJOINTS, typename SCALAR>
+const size_t& EndEffector<NJOINTS, SCALAR>::getLinkId()
+{
+ return linkId_;
+}
+
+template <size_t NJOINTS, typename SCALAR>
+void EndEffector<NJOINTS, SCALAR>::setLinkId(size_t linkId)
+{
+ linkId_ = linkId;
+}
+
+} /* namespace rbd */
+} /* namespace ct */
diff --git a/ct_rbd/include/ct/rbd/robot/kinematics/EndEffector.h b/ct_rbd/include/ct/rbd/robot/kinematics/EndEffector.h
new file mode 100644
index 0000000..3779766
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/kinematics/EndEffector.h
@@ -0,0 +1,46 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/rbd/state/JointState.h>
+#include <ct/rbd/state/RigidBodyPose.h>
+
+namespace ct {
+namespace rbd {
+
+template <size_t NJOINTS, typename SCALAR = double>
+class EndEffector
+{
+public:
+ typedef Eigen::Matrix<SCALAR, 6, NJOINTS> jacobian_t;
+ typedef typename JointState<NJOINTS>::Position joint_position_t;
+
+ EndEffector();
+
+ virtual ~EndEffector();
+
+ EndEffector(const EndEffector& other);
+
+ /**
+ * \brief Return the ID of the link to which the end-effector is rigidly attached to
+ * @return Link ID
+ */
+ const size_t& getLinkId();
+
+ /**
+ * \brief *DO NOT USE*. Set the link id on which an endeffector is on
+ * @param linkId LinkId to be set
+ */
+ void setLinkId(size_t linkId); // we should not have this public
+
+private:
+ // id of the link that the endeffector is on
+ size_t linkId_;
+};
+
+} /* namespace rbd */
+} /* namespace ct */
diff --git a/ct_rbd/include/ct/rbd/robot/kinematics/FloatingBaseTransforms.h b/ct_rbd/include/ct/rbd/robot/kinematics/FloatingBaseTransforms.h
new file mode 100644
index 0000000..b4d9f3f
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/kinematics/FloatingBaseTransforms.h
@@ -0,0 +1,29 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <memory>
+
+namespace ct {
+namespace rbd {
+
+template <class RBD>
+class FloatingBaseTransforms
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ FloatingBaseTransforms(std::shared_ptr<RBD> rbdContainer) : rbdContainer_(rbdContainer) {}
+ virtual ~FloatingBaseTransforms(){};
+
+
+private:
+ std::shared_ptr<RBD> rbdContainer_;
+};
+
+} /* namespace rbd */
+} /* namespace ct */
diff --git a/ct_rbd/include/ct/rbd/robot/kinematics/InverseKinematicsBase.h b/ct_rbd/include/ct/rbd/robot/kinematics/InverseKinematicsBase.h
new file mode 100644
index 0000000..8e5693e
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/kinematics/InverseKinematicsBase.h
@@ -0,0 +1,30 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/rbd/state/JointState.h>
+#include <ct/rbd/state/RigidBodyPose.h>
+
+namespace ct {
+namespace rbd {
+
+template <size_t NJOINTS, typename SCALAR = double>
+class InverseKinematicsBase
+{
+public:
+ virtual std::vector<typename tpl::JointState<NJOINTS, SCALAR>::Position> computeInverseKinematics(
+ const tpl::RigidBodyPose<SCALAR> &eeBasePose,
+ const std::vector<SCALAR> &freeJoints) const = 0;
+
+ virtual std::vector<typename tpl::JointState<NJOINTS, SCALAR>::Position> computeInverseKinematics(
+ const tpl::RigidBodyPose<SCALAR> &eeWorldPose,
+ const tpl::RigidBodyPose<SCALAR> &baseWorldPose,
+ const std::vector<SCALAR> &freeJoints) const = 0;
+};
+
+} /* namespace rbd */
+} /* namespace ct */
diff --git a/ct_rbd/include/ct/rbd/robot/kinematics/RBDDataMap.h b/ct_rbd/include/ct/rbd/robot/kinematics/RBDDataMap.h
new file mode 100644
index 0000000..c103c0d
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/kinematics/RBDDataMap.h
@@ -0,0 +1,111 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+/**
+ * @file RBDDataMap.h
+ * @author taken from iit rbd dynamics
+ */
+
+#ifndef _RBDDATAMAP_H_
+#define _RBDDATAMAP_H_
+
+#include <cstring>
+
+namespace ct {
+namespace rbd {
+
+/**
+ * \brief A very simple container to associate N generic data item T
+ */
+template <typename T, size_t N>
+class RBDDataMap
+{
+private:
+ T data[N];
+
+public:
+ RBDDataMap(){};
+ RBDDataMap(const T& defaultValue);
+ RBDDataMap(const RBDDataMap& rhs);
+ RBDDataMap& operator=(const RBDDataMap& rhs);
+ RBDDataMap& operator=(const T& rhs);
+ T& operator[](size_t which);
+ const T& operator[](size_t which) const;
+
+private:
+ void copydata(const RBDDataMap& rhs);
+ void assigndata(const T& commonValue);
+};
+
+template <typename T, size_t N>
+inline RBDDataMap<T, N>::RBDDataMap(const T& value)
+{
+ assigndata(value);
+}
+
+template <typename T, size_t N>
+inline RBDDataMap<T, N>::RBDDataMap(const RBDDataMap& rhs)
+{
+ copydata(rhs);
+}
+
+template <typename T, size_t N>
+inline RBDDataMap<T, N>& RBDDataMap<T, N>::operator=(const RBDDataMap& rhs)
+{
+ if (&rhs != this)
+ {
+ copydata(rhs);
+ }
+ return *this;
+}
+
+template <typename T, size_t N>
+inline RBDDataMap<T, N>& RBDDataMap<T, N>::operator=(const T& value)
+{
+ assigndata(value);
+ return *this;
+}
+
+template <typename T, size_t N>
+inline T& RBDDataMap<T, N>::operator[](size_t l)
+{
+ return data[l];
+}
+
+template <typename T, size_t N>
+inline const T& RBDDataMap<T, N>::operator[](size_t l) const
+{
+ return data[l];
+}
+
+template <typename T, size_t N>
+inline void RBDDataMap<T, N>::copydata(const RBDDataMap& rhs)
+{
+ for (size_t id = 0; id < N; ++id)
+ data[id] = rhs[id];
+}
+
+template <typename T, size_t N>
+inline void RBDDataMap<T, N>::assigndata(const T& value)
+{
+ for (size_t id = 0; id < N; ++id)
+ data[id] = value;
+}
+
+template <typename T, size_t N>
+inline std::ostream& operator<<(std::ostream& out, const RBDDataMap<T, N>& map)
+{
+ for (size_t id = 0; id < N; ++id)
+ {
+ out << "[" << id << "] = " << map[id] << " ";
+ }
+ return out;
+}
+
+} // namespace rbd
+} // namespace ct
+
+#endif /* _RBDDATAMAP_H_ */
diff --git a/ct_rbd/include/ct/rbd/robot/kinematics/ikfast/ikfast.h b/ct_rbd/include/ct/rbd/robot/kinematics/ikfast/ikfast.h
new file mode 100644
index 0000000..107d73c
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/kinematics/ikfast/ikfast.h
@@ -0,0 +1,340 @@
+// -*- coding: utf-8 -*-
+// Copyright (C) 2012-2014 Rosen Diankov <rosen.diankov@gmail.com>
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+/** \brief Header file for all ikfast c++ files/shared objects.
+
+ The ikfast inverse kinematics compiler is part of OpenRAVE.
+
+ The file is divided into two sections:
+ - <b>Common</b> - the abstract classes section that all ikfast share regardless of their settings
+ - <b>Library Specific</b> - the library-specific definitions, which depends on the precision/settings that the library was compiled with
+
+ The defines are as follows, they are also used for the ikfast C++ class:
+
+ - IKFAST_HEADER_COMMON - common classes
+ - IKFAST_HAS_LIBRARY - if defined, will include library-specific functions. by default this is off
+ - IKFAST_CLIBRARY - Define this linking statically or dynamically to get correct visibility.
+ - IKFAST_NO_MAIN - Remove the ``main`` function, usually used with IKFAST_CLIBRARY
+ - IKFAST_ASSERT - Define in order to get a custom assert called when NaNs, divides by zero, and other invalid conditions are detected.
+ - IKFAST_REAL - Use to force a custom real number type for IkReal.
+ - IKFAST_NAMESPACE - Enclose all functions and classes in this namespace, the ``main`` function is excluded.
+
+ */
+#include <vector>
+#include <list>
+#include <stdexcept>
+#include <cmath>
+
+#ifndef IKFAST_HEADER_COMMON
+#define IKFAST_HEADER_COMMON
+
+/// should be the same as ikfast.__version__
+/// if 0x10000000 bit is set, then the iksolver assumes 6D transforms are done without the manipulator offset taken into account (allows to reuse IK when manipulator offset changes)
+#define IKFAST_VERSION 0x10000048
+
+namespace ikfast {
+
+/// \brief holds the solution for a single dof
+template <typename T>
+class IkSingleDOFSolutionBase
+{
+public:
+ IkSingleDOFSolutionBase() : fmul(0), foffset(0), freeind(-1), maxsolutions(1) {
+ indices[0] = indices[1] = indices[2] = indices[3] = indices[4] = -1;
+ }
+ T fmul, foffset; ///< joint value is fmul*sol[freeind]+foffset
+ signed char freeind; ///< if >= 0, mimics another joint
+ unsigned char jointtype; ///< joint type, 0x01 is revolute, 0x11 is slider
+ unsigned char maxsolutions; ///< max possible indices, 0 if controlled by free index or a free joint itself
+ unsigned char indices[5]; ///< unique index of the solution used to keep track on what part it came from. sometimes a solution can be repeated for different indices. store at least another repeated root
+};
+
+/// \brief The discrete solutions are returned in this structure.
+///
+/// Sometimes the joint axes of the robot can align allowing an infinite number of solutions.
+/// Stores all these solutions in the form of free variables that the user has to set when querying the solution. Its prototype is:
+template <typename T>
+class IkSolutionBase
+{
+public:
+ virtual ~IkSolutionBase() {
+ }
+ /// \brief gets a concrete solution
+ ///
+ /// \param[out] solution the result
+ /// \param[in] freevalues values for the free parameters \se GetFree
+ virtual void GetSolution(T* solution, const T* freevalues) const = 0;
+
+ /// \brief std::vector version of \ref GetSolution
+ virtual void GetSolution(std::vector<T>& solution, const std::vector<T>& freevalues) const {
+ solution.resize(GetDOF());
+ GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL);
+ }
+
+ /// \brief Gets the indices of the configuration space that have to be preset before a full solution can be returned
+ ///
+ /// 0 always points to the first value accepted by the ik function.
+ /// \return vector of indices indicating the free parameters
+ virtual const std::vector<int>& GetFree() const = 0;
+
+ /// \brief the dof of the solution
+ virtual const int GetDOF() const = 0;
+};
+
+/// \brief manages all the solutions
+template <typename T>
+class IkSolutionListBase
+{
+public:
+ virtual ~IkSolutionListBase() {
+ }
+
+ /// \brief add one solution and return its index for later retrieval
+ ///
+ /// \param vinfos Solution data for each degree of freedom of the manipulator
+ /// \param vfree If the solution represents an infinite space, holds free parameters of the solution that users can freely set. The indices are of the configuration that the IK solver accepts rather than the entire robot, ie 0 points to the first value accepted.
+ virtual size_t AddSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree) = 0;
+
+ /// \brief returns the solution pointer
+ virtual const IkSolutionBase<T>& GetSolution(size_t index) const = 0;
+
+ /// \brief returns the number of solutions stored
+ virtual size_t GetNumSolutions() const = 0;
+
+ /// \brief clears all current solutions, note that any memory addresses returned from \ref GetSolution will be invalidated.
+ virtual void Clear() = 0;
+};
+
+/// \brief holds function pointers for all the exported functions of ikfast
+template <typename T>
+class IkFastFunctions
+{
+public:
+ IkFastFunctions() : _ComputeIk(NULL), _ComputeIk2(NULL), _ComputeFk(NULL), _GetNumFreeParameters(NULL), _GetFreeParameters(NULL), _GetNumJoints(NULL), _GetIkRealSize(NULL), _GetIkFastVersion(NULL), _GetIkType(NULL), _GetKinematicsHash(NULL) {
+ }
+ virtual ~IkFastFunctions() {
+ }
+ typedef bool (*ComputeIkFn)(const T*, const T*, const T*, IkSolutionListBase<T>&);
+ ComputeIkFn _ComputeIk;
+ typedef bool (*ComputeIk2Fn)(const T*, const T*, const T*, IkSolutionListBase<T>&, void*);
+ ComputeIk2Fn _ComputeIk2;
+ typedef void (*ComputeFkFn)(const T*, T*, T*);
+ ComputeFkFn _ComputeFk;
+ typedef int (*GetNumFreeParametersFn)();
+ GetNumFreeParametersFn _GetNumFreeParameters;
+ typedef int* (*GetFreeParametersFn)();
+ GetFreeParametersFn _GetFreeParameters;
+ typedef int (*GetNumJointsFn)();
+ GetNumJointsFn _GetNumJoints;
+ typedef int (*GetIkRealSizeFn)();
+ GetIkRealSizeFn _GetIkRealSize;
+ typedef const char* (*GetIkFastVersionFn)();
+ GetIkFastVersionFn _GetIkFastVersion;
+ typedef int (*GetIkTypeFn)();
+ GetIkTypeFn _GetIkType;
+ typedef const char* (*GetKinematicsHashFn)();
+ GetKinematicsHashFn _GetKinematicsHash;
+};
+
+// Implementations of the abstract classes, user doesn't need to use them
+
+/// \brief Default implementation of \ref IkSolutionBase
+template <typename T>
+class IkSolution : public IkSolutionBase<T>
+{
+public:
+ IkSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree) {
+ _vbasesol = vinfos;
+ _vfree = vfree;
+ }
+
+ virtual void GetSolution(T* solution, const T* freevalues) const {
+ for(std::size_t i = 0; i < _vbasesol.size(); ++i) {
+ if( _vbasesol[i].freeind < 0 )
+ solution[i] = _vbasesol[i].foffset;
+ else {
+ solution[i] = freevalues[_vbasesol[i].freeind]*_vbasesol[i].fmul + _vbasesol[i].foffset;
+ if( solution[i] > T(3.14159265358979) ) {
+ solution[i] -= T(6.28318530717959);
+ }
+ else if( solution[i] < T(-3.14159265358979) ) {
+ solution[i] += T(6.28318530717959);
+ }
+ }
+ }
+ }
+
+ virtual void GetSolution(std::vector<T>& solution, const std::vector<T>& freevalues) const {
+ solution.resize(GetDOF());
+ GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL);
+ }
+
+ virtual const std::vector<int>& GetFree() const {
+ return _vfree;
+ }
+ virtual const int GetDOF() const {
+ return static_cast<int>(_vbasesol.size());
+ }
+
+ virtual void Validate() const {
+ for(size_t i = 0; i < _vbasesol.size(); ++i) {
+ if( _vbasesol[i].maxsolutions == (unsigned char)-1) {
+ throw std::runtime_error("max solutions for joint not initialized");
+ }
+ if( _vbasesol[i].maxsolutions > 0 ) {
+ if( _vbasesol[i].indices[0] >= _vbasesol[i].maxsolutions ) {
+ throw std::runtime_error("index >= max solutions for joint");
+ }
+ if( _vbasesol[i].indices[1] != (unsigned char)-1 && _vbasesol[i].indices[1] >= _vbasesol[i].maxsolutions ) {
+ throw std::runtime_error("2nd index >= max solutions for joint");
+ }
+ }
+ if( !std::isfinite(_vbasesol[i].foffset) ) {
+ throw std::runtime_error("foffset was not finite");
+ }
+ }
+ }
+
+ virtual void GetSolutionIndices(std::vector<unsigned int>& v) const {
+ v.resize(0);
+ v.push_back(0);
+ for(int i = (int)_vbasesol.size()-1; i >= 0; --i) {
+ if( _vbasesol[i].maxsolutions != (unsigned char)-1 && _vbasesol[i].maxsolutions > 1 ) {
+ for(size_t j = 0; j < v.size(); ++j) {
+ v[j] *= _vbasesol[i].maxsolutions;
+ }
+ size_t orgsize=v.size();
+ if( _vbasesol[i].indices[1] != (unsigned char)-1 ) {
+ for(size_t j = 0; j < orgsize; ++j) {
+ v.push_back(v[j]+_vbasesol[i].indices[1]);
+ }
+ }
+ if( _vbasesol[i].indices[0] != (unsigned char)-1 ) {
+ for(size_t j = 0; j < orgsize; ++j) {
+ v[j] += _vbasesol[i].indices[0];
+ }
+ }
+ }
+ }
+ }
+
+ std::vector< IkSingleDOFSolutionBase<T> > _vbasesol; ///< solution and their offsets if joints are mimiced
+ std::vector<int> _vfree;
+};
+
+/// \brief Default implementation of \ref IkSolutionListBase
+template <typename T>
+class IkSolutionList : public IkSolutionListBase<T>
+{
+public:
+ virtual size_t AddSolution(const std::vector<IkSingleDOFSolutionBase<T> >& vinfos, const std::vector<int>& vfree)
+ {
+ size_t index = _listsolutions.size();
+ _listsolutions.push_back(IkSolution<T>(vinfos,vfree));
+ return index;
+ }
+
+ virtual const IkSolutionBase<T>& GetSolution(size_t index) const
+ {
+ if( index >= _listsolutions.size() ) {
+ throw std::runtime_error("GetSolution index is invalid");
+ }
+ typename std::list< IkSolution<T> >::const_iterator it = _listsolutions.begin();
+ std::advance(it,index);
+ return *it;
+ }
+
+ virtual size_t GetNumSolutions() const {
+ return _listsolutions.size();
+ }
+
+ virtual void Clear() {
+ _listsolutions.clear();
+ }
+
+protected:
+ std::list< IkSolution<T> > _listsolutions;
+};
+
+}
+
+#endif // OPENRAVE_IKFAST_HEADER
+
+// The following code is dependent on the C++ library linking with.
+#ifdef IKFAST_HAS_LIBRARY
+
+// defined when creating a shared object/dll
+#ifdef IKFAST_CLIBRARY
+#ifdef _MSC_VER
+#define IKFAST_API extern "C" __declspec(dllexport)
+#else
+#define IKFAST_API extern "C"
+#endif
+#else
+#define IKFAST_API
+#endif
+
+#ifdef IKFAST_NAMESPACE
+namespace IKFAST_NAMESPACE {
+#endif
+
+#ifdef IKFAST_REAL
+typedef IKFAST_REAL IkReal;
+#else
+typedef double IkReal;
+#endif
+
+/** \brief Computes all IK solutions given a end effector coordinates and the free joints.
+
+ - ``eetrans`` - 3 translation values. For iktype **TranslationXYOrientation3D**, the z-axis is the orientation.
+ - ``eerot``
+ - For **Transform6D** it is 9 values for the 3x3 rotation matrix.
+ - For **Direction3D**, **Ray4D**, and **TranslationDirection5D**, the first 3 values represent the target direction.
+ - For **TranslationXAxisAngle4D**, **TranslationYAxisAngle4D**, and **TranslationZAxisAngle4D** the first value represents the angle.
+ - For **TranslationLocalGlobal6D**, the diagonal elements ([0],[4],[8]) are the local translation inside the end effector coordinate system.
+ */
+IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, ikfast::IkSolutionListBase<IkReal>& solutions);
+
+/** \brief Similar to ComputeIk except takes OpenRAVE boost::shared_ptr<RobotBase::Manipulator>*
+ */
+IKFAST_API bool ComputeIk2(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, ikfast::IkSolutionListBase<IkReal>& solutions, void* pOpenRAVEManip);
+
+/// \brief Computes the end effector coordinates given the joint values. This function is used to double check ik.
+IKFAST_API void ComputeFk(const IkReal* joints, IkReal* eetrans, IkReal* eerot);
+
+/// \brief returns the number of free parameters users has to set apriori
+IKFAST_API int GetNumFreeParameters();
+
+/// \brief the indices of the free parameters indexed by the chain joints
+IKFAST_API int* GetFreeParameters();
+
+/// \brief the total number of indices of the chain
+IKFAST_API int GetNumJoints();
+
+/// \brief the size in bytes of the configured number type
+IKFAST_API int GetIkRealSize();
+
+/// \brief the ikfast version used to generate this file
+IKFAST_API const char* GetIkFastVersion();
+
+/// \brief the ik type ID
+IKFAST_API int GetIkType();
+
+/// \brief a hash of all the chain values used for double checking that the correct IK is used.
+IKFAST_API const char* GetKinematicsHash();
+
+#ifdef IKFAST_NAMESPACE
+}
+#endif
+
+#endif // IKFAST_HAS_LIBRARY
diff --git a/ct_rbd/include/ct/rbd/robot/robcogen/robcogenHelpers.h b/ct_rbd/include/ct/rbd/robot/robcogen/robcogenHelpers.h
new file mode 100644
index 0000000..7bbc192
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/robot/robcogen/robcogenHelpers.h
@@ -0,0 +1,930 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#ifdef CT_L23
+#error You specified 23 links but the maximum supported is 22.
+#endif
+
+#ifdef CT_EE23
+#error You specified 23 end-effectors but the maximum supported is 22.
+#endif
+
+#ifndef ROBCOGEN_NS
+#error Please specify the NS of the robot in RobCoGen (ROBCOGEN_NS)
+#endif
+
+#ifndef TARGET_NS
+#error Please specify a target NS (TARGET_NS)
+#endif
+
+#ifndef CT_BASE
+#error Please specify base name (CT_BASE)
+#endif
+
+#ifndef CT_N_EE
+#error Please specify the number of endeffectors (CT_N_EE)
+#endif
+
+#define CT_RBD_TRANSFORM_BASE_TO_ID(BASE, LINK_ID) transforms.BASE##_X_##LINK_ID
+
+#define CT_RBD_JACOBIAN_BASE_TO_ID(BASE, EE_ID) jacobians.BASE##_J_##EE_ID
+
+#define CT_RBD_JACOBIAN_GET_BLOCK(FIRST, LAST) jacobian.template block<6, LAST - FIRST + 1>(0, FIRST)
+
+// This is just a helper Macro that generates a case-statement for each link to shorten the macro below.
+#define CT_RBD_CASE_HELPER_BASE_ID(BASE, LINK_ID, INDEX) \
+ case INDEX: \
+ return CT_RBD_TRANSFORM_BASE_TO_ID(BASE, LINK_ID); \
+ break;
+
+// This is just a helper Macro that generates a case-statement for each link to shorten the macro below.
+#define CT_RBD_CASE_HELPER_JAC_UPDATE(BASE, LINK_ID) CT_RBD_JACOBIAN_BASE_TO_ID(BASE, LINK_ID).update(q);
+
+// This is just a helper Macro that generates a case-statement for each link to shorten the macro below.
+#define CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(BASE, LINK_ID, INDEX) \
+ case INDEX: \
+ return CT_RBD_TRANSFORM_BASE_TO_ID(BASE, LINK_ID).update(
+
+#define CT_RBD_TRANSFORM_ID_TO_BASE(BASE, LINK_ID) transforms.LINK_ID##_X_##BASE
+
+// This is just a helper Macro that generates a case-statement for each link to shorten the macro below.
+#define CT_RBD_CASE_HELPER_ID_BASE(BASE, LINK_ID, INDEX) \
+ case INDEX: \
+ return CT_RBD_TRANSFORM_ID_TO_BASE(BASE, LINK_ID); \
+ break;
+
+// This is just a helper Macro that generates a case-statement for each link to shorten the macro below.
+#define CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(BASE, LINK_ID, INDEX) \
+ case INDEX: \
+ return CT_RBD_TRANSFORM_ID_TO_BASE(BASE, LINK_ID).update(
+
+#define CT_RBD_CASE_HELPER_JOINT_END \
+ ) ; \
+ break;
+
+
+namespace ct {
+namespace rbd {
+namespace TARGET_NS {
+
+template <typename SCALAR>
+class Utils
+{
+private:
+ static const int NJOINTS = iit::ROBCOGEN_NS::tpl::Traits<SCALAR>::joints_count;
+
+public:
+ static const int N_EE = CT_N_EE;
+
+ // This defines a function to get the transform by ID
+ template <class TRANS>
+ static typename TRANS::MatrixType getTransformBaseLinkById(TRANS& transforms,
+ size_t link_id,
+ const Eigen::Matrix<SCALAR, NJOINTS, 1>& q)
+ {
+ switch (link_id)
+ {
+ case 0:
+ {
+ typename TRANS::MatrixType identity;
+ identity.setZero();
+ identity.template topRightCorner<3, 3>().setIdentity();
+ return identity;
+ break;
+ }
+#ifdef CT_L0
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(CT_BASE, CT_L0, 0 + 1)
+ q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L1
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(CT_BASE, CT_L1, 1 + 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L2
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(CT_BASE, CT_L2, 2 + 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L3
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_L3, 3 + 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L4
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_L4, 4 + 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L5
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_L5, 5 + 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L6
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_L6, 6 + 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L7
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_L7, 7 + 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L8
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_L8, 8 + 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L9
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_L9, 9 + 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L10
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_L10, 10 + 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L11
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(CT_BASE, CT_L11,
+ 11 + 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L12
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(CT_BASE,
+ CT_L12, 12 + 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L13
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(CT_BASE,
+ CT_L13,
+ 13 + 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L14
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_L14,
+ 14 + 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L15
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_L15,
+ 15 +
+ 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L16
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_L16,
+ 16 +
+ 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L17
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_L17,
+ 17 +
+ 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L18
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_L18,
+ 18 +
+ 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L19
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_L19,
+ 19 +
+ 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L20
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_L20,
+ 20 +
+ 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L21
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE,
+ CT_L21,
+ 21 +
+ 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L22
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE,
+ CT_L22,
+ 22 +
+ 1) q
+ CT_RBD_CASE_HELPER_JOINT_END
+#endif
+ default : std::cout
+ << "getTransformByLinkID: requested joint does not exist, requested: "
+ << link_id
+ << std::endl;
+ throw std::runtime_error("getTransformByLinkID: requested joint does not exist");
+ break;
+ }
+ }
+
+
+ // This defines a function to get the transform by ID
+ template <class TRANS>
+ static typename TRANS::MatrixType getTransformLinkBaseById(TRANS& transforms,
+ size_t link_id,
+ const Eigen::Matrix<SCALAR, NJOINTS, 1>& q)
+ {
+ switch (link_id)
+ {
+ case 0:
+ {
+ typename TRANS::MatrixType identity;
+ identity.setZero();
+ identity.template topRightCorner<3, 3>().setIdentity();
+ return identity;
+ break;
+ }
+#ifdef CT_L0
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(CT_BASE, CT_L0, 0 + 1)
+ q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L1
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(CT_BASE, CT_L1, 1 + 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L2
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(CT_BASE, CT_L2, 2 + 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L3
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
+ CT_BASE, CT_L3, 3 + 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L4
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
+ CT_BASE, CT_L4, 4 + 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L5
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
+ CT_BASE, CT_L5, 5 + 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L6
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
+ CT_BASE, CT_L6, 6 + 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L7
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
+ CT_BASE, CT_L7, 7 + 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L8
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
+ CT_BASE, CT_L8, 8 + 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L9
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
+ CT_BASE, CT_L9, 9 + 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L10
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
+ CT_BASE, CT_L10, 10 + 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L11
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(CT_BASE, CT_L11,
+ 11 + 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L12
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(CT_BASE,
+ CT_L12, 12 + 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L13
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(CT_BASE,
+ CT_L13,
+ 13 + 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L14
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
+ CT_BASE, CT_L14,
+ 14 + 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L15
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
+ CT_BASE, CT_L15,
+ 15 +
+ 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L16
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
+ CT_BASE, CT_L16,
+ 16 +
+ 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L17
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
+ CT_BASE, CT_L17,
+ 17 +
+ 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L18
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
+ CT_BASE, CT_L18,
+ 18 +
+ 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L19
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
+ CT_BASE, CT_L19,
+ 19 +
+ 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L20
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
+ CT_BASE, CT_L20,
+ 20 +
+ 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L21
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
+ CT_BASE,
+ CT_L21,
+ 21 +
+ 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_L22
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
+ CT_BASE,
+ CT_L22,
+ 22 +
+ 1) q
+ CT_RBD_CASE_HELPER_JOINT_END
+#endif
+ default : std::cout
+ << "getTransformByLinkID: requested joint does not exist, requested: "
+ << link_id
+ << std::endl;
+ throw std::runtime_error("getTransformByLinkID: requested joint does not exist");
+ break;
+ }
+ }
+
+
+ // This defines a function to get the transform by ID
+ template <class TRANS>
+ static typename TRANS::MatrixType getTransformBaseEEById(TRANS& transforms,
+ size_t ee_id,
+ const Eigen::Matrix<SCALAR, NJOINTS, 1>& q)
+ {
+ switch (ee_id)
+ {
+#ifdef CT_EE0
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(CT_BASE, CT_EE0, 0)
+ q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_EE1
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(CT_BASE, CT_EE1, 1) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_EE2
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(CT_BASE, CT_EE2, 2) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_EE3
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(CT_BASE, CT_EE3, 3) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_EE4
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_EE4, 4) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_EE5
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_EE5, 5) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_EE6
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_EE6, 6) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_EE7
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_EE7, 7) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_EE8
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_EE8, 8) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_EE9
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_EE9, 9) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_EE10
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_EE10, 10) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_EE11
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_EE11, 11) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_EE12
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_EE12, 12) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_EE13
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(CT_BASE,
+ CT_EE13, 13) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_EE14
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(CT_BASE,
+ CT_EE14, 14) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_EE15
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_EE15,
+ 15) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_EE16
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_EE16,
+ 16) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_EE17
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_EE17,
+ 17) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_EE18
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_EE18,
+ 18) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_EE19
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_EE19,
+ 19) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_EE20
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE, CT_EE20,
+ 20) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_EE21
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE,
+ CT_EE21,
+ 21) q CT_RBD_CASE_HELPER_JOINT_END
+#endif
+#ifdef CT_EE22
+ CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
+ CT_BASE,
+ CT_EE22,
+ 22) q
+ CT_RBD_CASE_HELPER_JOINT_END
+#endif
+ default : std::cout
+ << "getTransformByEEID: requested end-effector does not exist, requested: "
+ << ee_id
+ << std::endl;
+ throw std::runtime_error("getTransformByEEID: requested end-effector does not exist");
+ break;
+ }
+ }
+
+ static size_t eeIdToLinkId(size_t ee_id)
+ {
+ switch (ee_id)
+ {
+#ifdef CT_EE0
+ case 0:
+ return CT_EE0_IS_ON_LINK;
+ break;
+#endif
+#ifdef CT_EE1
+ case 1:
+ return CT_EE1_IS_ON_LINK;
+ break;
+#endif
+#ifdef CT_EE2
+ case 2:
+ return CT_EE2_IS_ON_LINK;
+ break;
+#endif
+#ifdef CT_EE3
+ case 3:
+ return CT_EE3_IS_ON_LINK;
+ break;
+#endif
+#ifdef CT_EE4
+ case 4:
+ return CT_EE4_IS_ON_LINK;
+ break;
+#endif
+#ifdef CT_EE5
+ case 5:
+ return CT_EE5_IS_ON_LINK;
+ break;
+#endif
+#ifdef CT_EE6
+ case 6:
+ return CT_EE6_IS_ON_LINK;
+ break;
+#endif
+#ifdef CT_EE7
+ case 7:
+ return CT_EE7_IS_ON_LINK;
+ break;
+#endif
+#ifdef CT_EE8
+ case 8:
+ return CT_EE8_IS_ON_LINK;
+ break;
+#endif
+#ifdef CT_EE9
+ case 9:
+ return CT_EE9_IS_ON_LINK;
+ break;
+#endif
+#ifdef CT_EE10
+ case 10:
+ return CT_EE10_IS_ON_LINK;
+ break;
+#endif
+#ifdef CT_EE11
+ case 11:
+ return CT_EE11_IS_ON_LINK;
+ break;
+#endif
+#ifdef CT_EE12
+ case 12:
+ return CT_EE12_IS_ON_LINK;
+ break;
+#endif
+#ifdef CT_EE13
+ case 13:
+ return CT_EE13_IS_ON_LINK;
+ break;
+#endif
+#ifdef CT_EE14
+ case 14:
+ return CT_EE14_IS_ON_LINK;
+ break;
+#endif
+#ifdef CT_EE15
+ case 15:
+ return CT_EE15_IS_ON_LINK;
+ break;
+#endif
+#ifdef CT_EE16
+ case 16:
+ return CT_EE16_IS_ON_LINK;
+ break;
+#endif
+#ifdef CT_EE17
+ case 17:
+ return CT_EE17_IS_ON_LINK;
+ break;
+#endif
+#ifdef CT_EE18
+ case 18:
+ return CT_EE18_IS_ON_LINK;
+ break;
+#endif
+#ifdef CT_EE19
+ case 19:
+ return CT_EE19_IS_ON_LINK;
+ break;
+#endif
+#ifdef CT_EE20
+ case 20:
+ return CT_EE20_IS_ON_LINK;
+ break;
+#endif
+#ifdef CT_EE21
+ case 21:
+ return CT_EE21_IS_ON_LINK;
+ break;
+#endif
+#ifdef CT_EE22
+ case 22:
+ return CT_EE22_IS_ON_LINK;
+ break;
+#endif
+
+ default:
+ std::cout << "eeIdToLinkId: requested end-effector does not exist, requested: " << ee_id << std::endl;
+ throw std::runtime_error("getTransformByEEID: requested end-effector does not exist");
+ break;
+ }
+ }
+
+
+ // This defines a function to get the transform by ID
+ template <class JACS>
+ static typename Eigen::Matrix<SCALAR, 6, NJOINTS> getJacobianBaseEEbyId(JACS& jacobians,
+ size_t ee_id,
+ const Eigen::Matrix<SCALAR, NJOINTS, 1>& q)
+ {
+ Eigen::Matrix<SCALAR, 6, NJOINTS> jacobian(Eigen::Matrix<SCALAR, 6, NJOINTS>::Zero());
+
+ switch (ee_id)
+ {
+#ifdef CT_EE0
+ case 0:
+ CT_RBD_JACOBIAN_GET_BLOCK(CT_EE0_FIRST_JOINT, CT_EE0_LAST_JOINT) =
+ CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE0);
+ break;
+#endif
+#ifdef CT_EE1
+ case 1:
+ CT_RBD_JACOBIAN_GET_BLOCK(CT_EE1_FIRST_JOINT, CT_EE1_LAST_JOINT) =
+ CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE1);
+ break;
+#endif
+#ifdef CT_EE2
+ case 2:
+ CT_RBD_JACOBIAN_GET_BLOCK(CT_EE2_FIRST_JOINT, CT_EE2_LAST_JOINT) =
+ CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE2);
+ break;
+#endif
+#ifdef CT_EE3
+ case 3:
+ CT_RBD_JACOBIAN_GET_BLOCK(CT_EE3_FIRST_JOINT, CT_EE3_LAST_JOINT) =
+ CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE3);
+ break;
+#endif
+#ifdef CT_EE4
+ case 4:
+ CT_RBD_JACOBIAN_GET_BLOCK(CT_EE4_FIRST_JOINT, CT_EE4_LAST_JOINT) =
+ CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE4);
+ break;
+#endif
+#ifdef CT_EE5
+ case 5:
+ CT_RBD_JACOBIAN_GET_BLOCK(CT_EE5_FIRST_JOINT, CT_EE5_LAST_JOINT) =
+ CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE5);
+ break;
+#endif
+#ifdef CT_EE6
+ case 6:
+ CT_RBD_JACOBIAN_GET_BLOCK(CT_EE6_FIRST_JOINT, CT_EE6_LAST_JOINT) =
+ CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE6);
+ break;
+#endif
+#ifdef CT_EE7
+ case 7:
+ CT_RBD_JACOBIAN_GET_BLOCK(CT_EE7_FIRST_JOINT, CT_EE7_LAST_JOINT) =
+ CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE7);
+ break;
+#endif
+#ifdef CT_EE8
+ case 8:
+ CT_RBD_JACOBIAN_GET_BLOCK(CT_EE8_FIRST_JOINT, CT_EE8_LAST_JOINT) =
+ CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE8);
+ break;
+#endif
+#ifdef CT_EE9
+ case 9:
+ CT_RBD_JACOBIAN_GET_BLOCK(CT_EE9_FIRST_JOINT, CT_EE9_LAST_JOINT) =
+ CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE9);
+ break;
+#endif
+#ifdef CT_EE10
+ case 10:
+ CT_RBD_JACOBIAN_GET_BLOCK(CT_EE10_FIRST_JOINT, CT_EE10_LAST_JOINT) =
+ CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE10);
+ break;
+#endif
+#ifdef CT_EE11
+ case 11:
+ CT_RBD_JACOBIAN_GET_BLOCK(CT_EE11_FIRST_JOINT, CT_EE11_LAST_JOINT) =
+ CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE11);
+ break;
+#endif
+#ifdef CT_EE12
+ case 12:
+ CT_RBD_JACOBIAN_GET_BLOCK(CT_EE12_FIRST_JOINT, CT_EE12_LAST_JOINT) =
+ CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE12);
+ break;
+#endif
+#ifdef CT_EE13
+ case 13:
+ CT_RBD_JACOBIAN_GET_BLOCK(CT_EE13_FIRST_JOINT, CT_EE13_LAST_JOINT) =
+ CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE13);
+ break;
+#endif
+#ifdef CT_EE14
+ case 14:
+ CT_RBD_JACOBIAN_GET_BLOCK(CT_EE14_FIRST_JOINT, CT_EE14_LAST_JOINT) =
+ CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE14);
+ break;
+#endif
+#ifdef CT_EE15
+ case 15:
+ CT_RBD_JACOBIAN_GET_BLOCK(CT_EE15_FIRST_JOINT, CT_EE15_LAST_JOINT) =
+ CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE15);
+ break;
+#endif
+#ifdef CT_EE16
+ case 16:
+ CT_RBD_JACOBIAN_GET_BLOCK(CT_EE16_FIRST_JOINT, CT_EE16_LAST_JOINT) =
+ CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE16);
+ break;
+#endif
+#ifdef CT_EE17
+ case 17:
+ CT_RBD_JACOBIAN_GET_BLOCK(CT_EE17_FIRST_JOINT, CT_EE17_LAST_JOINT) =
+ CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE17);
+ break;
+#endif
+#ifdef CT_EE18
+ case 18:
+ CT_RBD_JACOBIAN_GET_BLOCK(CT_EE18_FIRST_JOINT, CT_EE18_LAST_JOINT) =
+ CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE18);
+ break;
+#endif
+#ifdef CT_EE19
+ case 19:
+ CT_RBD_JACOBIAN_GET_BLOCK(CT_EE19_FIRST_JOINT, CT_EE19_LAST_JOINT) =
+ CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE19);
+ break;
+#endif
+#ifdef CT_EE20
+ case 20:
+ CT_RBD_JACOBIAN_GET_BLOCK(CT_EE20_FIRST_JOINT, CT_EE20_LAST_JOINT) =
+ CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE20);
+ break;
+#endif
+#ifdef CT_EE21
+ case 21:
+ CT_RBD_JACOBIAN_GET_BLOCK(CT_EE21_FIRST_JOINT, CT_EE21_LAST_JOINT) =
+ CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE21);
+ break;
+#endif
+#ifdef CT_EE22
+ case 22:
+ CT_RBD_JACOBIAN_GET_BLOCK(CT_EE22_FIRST_JOINT, CT_EE22_LAST_JOINT) =
+ CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE22);
+ break;
+#endif
+
+ default:
+ std::cout << "getJacobianBaseByEEID: requested end-effector does not exist, requested: " << ee_id
+ << std::endl;
+ throw std::runtime_error("getJacobianBaseByEEID: requested end-effector does not exist");
+ break;
+ }
+
+ return jacobian;
+ }
+
+}; // class Utils
+
+
+namespace tpl {
+
+template <typename SCALAR>
+using RobCoGenContainer =
+ ct::rbd::RobCoGenContainer<iit::ROBCOGEN_NS::tpl::Traits<SCALAR>, iit::ROBCOGEN_NS::LinkDataMap, Utils<SCALAR>>;
+
+template <typename SCALAR>
+using Kinematics = ct::rbd::Kinematics<RobCoGenContainer<SCALAR>, Utils<SCALAR>::N_EE>;
+
+template <typename SCALAR>
+using Dynamics = ct::rbd::Dynamics<RobCoGenContainer<SCALAR>, Utils<SCALAR>::N_EE>;
+
+
+} // namespace tpl
+
+
+typedef tpl::RobCoGenContainer<double> RobCoGenContainer;
+typedef tpl::Kinematics<double> Kinematics;
+typedef Dynamics<RobCoGenContainer, Kinematics::NUM_EE> Dynamics;
+
+} // TARGET_NS
+
+
+} // namespace rbd
+
+} // namespace ct
+
+// CLEAN UP
+
+#undef CT_BASE
+#undef ROBCOGEN_NS
+#undef ROBOT_NAME
+#undef TARGET_NS
+#undef CT_N_EE
+
+#undef CT_L0
+#undef CT_L1
+#undef CT_L2
+#undef CT_L3
+#undef CT_L4
+#undef CT_L5
+#undef CT_L6
+#undef CT_L7
+#undef CT_L8
+#undef CT_L9
+#undef CT_L10
+#undef CT_L11
+#undef CT_L12
+#undef CT_L13
+#undef CT_L14
+#undef CT_L15
+#undef CT_L16
+#undef CT_L17
+#undef CT_L18
+#undef CT_L19
+#undef CT_L20
+#undef CT_L21
+#undef CT_L22
+
+#undef CT_EE0
+#undef CT_EE1
+#undef CT_EE2
+#undef CT_EE3
+#undef CT_EE4
+#undef CT_EE5
+#undef CT_EE6
+#undef CT_EE7
+#undef CT_EE8
+#undef CT_EE9
+#undef CT_EE10
+#undef CT_EE11
+#undef CT_EE12
+#undef CT_EE13
+#undef CT_EE14
+#undef CT_EE15
+#undef CT_EE16
+#undef CT_EE17
+#undef CT_EE18
+#undef CT_EE19
+#undef CT_EE20
+#undef CT_EE21
+#undef CT_EE22
+
+#undef CT_EE0_IS_ON_LINK
+#undef CT_EE1_IS_ON_LINK
+#undef CT_EE2_IS_ON_LINK
+#undef CT_EE3_IS_ON_LINK
+#undef CT_EE4_IS_ON_LINK
+#undef CT_EE5_IS_ON_LINK
+#undef CT_EE6_IS_ON_LINK
+#undef CT_EE7_IS_ON_LINK
+#undef CT_EE8_IS_ON_LINK
+#undef CT_EE9_IS_ON_LINK
+#undef CT_EE10_IS_ON_LINK
+#undef CT_EE11_IS_ON_LINK
+#undef CT_EE12_IS_ON_LINK
+#undef CT_EE13_IS_ON_LINK
+#undef CT_EE14_IS_ON_LINK
+#undef CT_EE15_IS_ON_LINK
+#undef CT_EE16_IS_ON_LINK
+#undef CT_EE17_IS_ON_LINK
+#undef CT_EE18_IS_ON_LINK
+#undef CT_EE19_IS_ON_LINK
+#undef CT_EE20_IS_ON_LINK
+#undef CT_EE21_IS_ON_LINK
+#undef CT_EE22_IS_ON_LINK
+
+#undef CT_EE0_FIRST_JOINT
+#undef CT_EE1_FIRST_JOINT
+#undef CT_EE2_FIRST_JOINT
+#undef CT_EE3_FIRST_JOINT
+#undef CT_EE4_FIRST_JOINT
+#undef CT_EE5_FIRST_JOINT
+#undef CT_EE6_FIRST_JOINT
+#undef CT_EE7_FIRST_JOINT
+#undef CT_EE8_FIRST_JOINT
+#undef CT_EE9_FIRST_JOINT
+#undef CT_EE10_FIRST_JOINT
+#undef CT_EE11_FIRST_JOINT
+#undef CT_EE12_FIRST_JOINT
+#undef CT_EE13_FIRST_JOINT
+#undef CT_EE14_FIRST_JOINT
+#undef CT_EE15_FIRST_JOINT
+#undef CT_EE16_FIRST_JOINT
+#undef CT_EE17_FIRST_JOINT
+#undef CT_EE18_FIRST_JOINT
+#undef CT_EE19_FIRST_JOINT
+#undef CT_EE20_FIRST_JOINT
+#undef CT_EE21_FIRST_JOINT
+#undef CT_EE22_FIRST_JOINT
+
+
+#undef CT_EE0_LAST_JOINT
+#undef CT_EE1_LAST_JOINT
+#undef CT_EE2_LAST_JOINT
+#undef CT_EE3_LAST_JOINT
+#undef CT_EE4_LAST_JOINT
+#undef CT_EE5_LAST_JOINT
+#undef CT_EE6_LAST_JOINT
+#undef CT_EE7_LAST_JOINT
+#undef CT_EE8_LAST_JOINT
+#undef CT_EE9_LAST_JOINT
+#undef CT_EE10_LAST_JOINT
+#undef CT_EE11_LAST_JOINT
+#undef CT_EE12_LAST_JOINT
+#undef CT_EE13_LAST_JOINT
+#undef CT_EE14_LAST_JOINT
+#undef CT_EE15_LAST_JOINT
+#undef CT_EE16_LAST_JOINT
+#undef CT_EE17_LAST_JOINT
+#undef CT_EE18_LAST_JOINT
+#undef CT_EE19_LAST_JOINT
+#undef CT_EE20_LAST_JOINT
+#undef CT_EE21_LAST_JOINT
+#undef CT_EE22_LAST_JOINT
+
+
+#undef CT_RBD_TRANSFORM_BASE_TO_ID
+#undef CT_RBD_JACOBIAN_BASE_TO_ID
+#undef CT_RBD_JACOBIAN_GET_BLOCK
+#undef CT_RBD_CASE_HELPER_BASE_ID
+#undef CT_RBD_CASE_HELPER_JAC_UPDATE
+#undef CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID
+#undef CT_RBD_TRANSFORM_ID_TO_BASE
+#undef CT_RBD_CASE_HELPER_ID_BASE
+#undef CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE
+#undef CT_RBD_CASE_HELPER_JOINT_END
diff --git a/ct_rbd/include/ct/rbd/slq/FloatingBaseSLQ.h b/ct_rbd/include/ct/rbd/slq/FloatingBaseSLQ.h
new file mode 100644
index 0000000..3778a16
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/slq/FloatingBaseSLQ.h
@@ -0,0 +1,114 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#ifndef INCLUDE_CT_RBD_SLQ_FLOATINGBASESLQ_H_
+#define INCLUDE_CT_RBD_SLQ_FLOATINGBASESLQ_H_
+
+#include <ct/core/systems/linear/SystemLinearizer.h>
+
+#include <ct/optcon/ilqg/iLQGMP.hpp>
+
+#include <ct/rbd/systems/FloatingBaseFDSystem.h>
+
+
+namespace ct {
+namespace rbd {
+
+/**
+ * \brief SLQ for floating base systems without an explicit contact model.
+ * The contact constraint is enforced via a cost function
+ */
+template <class RBDDynamics>
+class FloatingBaseSLQ
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ static const bool quatIntegration = false;
+ static const bool eeForcesAreControlInputs = true;
+
+ typedef FloatingBaseFDSystem<RBDDynamics, false, true> FBSystem;
+ typedef ct::core::LinearSystem<FBSystem::STATE_DIM, FBSystem::CONTROL_DIM> LinearizedSystem;
+ typedef ct::core::SystemLinearizer<FBSystem::STATE_DIM, FBSystem::CONTROL_DIM> SystemLinearizer;
+
+ typedef ct::optcon::iLQGMP<FBSystem::STATE_DIM, FBSystem::CONTROL_DIM> iLQG;
+ typedef ct::optcon::iLQGMP<FBSystem::STATE_DIM, FBSystem::CONTROL_DIM> iLQGMP;
+
+ typedef ct::optcon::CostFunctionAnalytical<FBSystem::STATE_DIM, FBSystem::CONTROL_DIM> CostFunction;
+ typedef ct::rbd::TermTaskspace<FBSystem::Kinematics, true, FBSystem::STATE_DIM, FBSystem::CONTROL_DIM>
+ TaskSpaceTerm;
+
+ typedef iLQG::StateTrajectory StateTrajectory;
+
+
+ FloatingBaseSLQ(std::shared_ptr<FBSystem> system,
+ const std::string& costFunctionFile,
+ std::shared_ptr<LinearizedSystem> linearizedSystem = nullptr,
+ bool useMP = true)
+ : system_(system), linearizedSystem_(linearizedSystem), useMP_(useMP), iteration_(0)
+ {
+ // if no linearized system provided, use general system linearizer
+ if (linearizedSystem_ == nullptr)
+ linearizedSystem_ = std::shared_ptr<LinearizedSystem>(new SystemLinearizer(system));
+
+ setupCostFunction(costFunctionFile);
+
+ ilqg_ = std::shared_ptr<iLQG>(new iLQG(system_, linearizedSystem_, costFunction_));
+ ilqgMp_ = std::shared_ptr<iLQGMP>(new iLQGMP(system_, linearizedSystem_, costFunction_));
+ }
+
+ bool runIteration(StateTrajectory& stateTrajectory)
+ {
+ bool foundBetter;
+
+ if (useMP_)
+ {
+ foundBetter = ilqgMp_->runIteration(ilqg_settings_, line_search_settings_);
+ stateTrajectory = ilqgMp_->retrieveLastRollout();
+ }
+ else
+ {
+ foundBetter = ilqg_->runIteration(ilqg_settings_, line_search_settings_);
+ stateTrajectory = ilqg_->retrieveLastRollout();
+ }
+
+ iteration_++;
+ return foundBetter;
+ }
+
+ ilqg_settings_t iLQGSettings() { return ilqg_settings_; }
+private:
+ void setupCostFunction(const std::string& filename)
+ {
+ costFunction_ = std::shared_ptr<CostFunction>(new CostFunction(filename, true));
+
+ for (size_t i = 0; i < N_EE; i++)
+ {
+ eePosTerms_.push_back(new TaskSpaceTerm(filename, "eePos" + std::to_string(i), true));
+ costFunction_->addIntermediateTerm(eePosTerms_.back());
+ }
+ }
+
+ std::shared_ptr<FBSystem> system_;
+ std::shared_ptr<LinearizedSystem> linearizedSystem_;
+
+ std::shared_ptr<iLQG> ilqg_;
+ std::shared_ptr<iLQG> ilqgMp_;
+
+ std::shared_ptr<CostFunction> costFunction_;
+ std::vector<std::shared_ptr<TaskSpaceTerm>> eePosTerms_;
+
+ ilqg_settings_t ilqg_settings_;
+ line_search_settings_t line_search_settings_;
+
+ bool useMP_;
+
+ size_t iteration_;
+};
+}
+}
+
+#endif /* INCLUDE_CT_RBD_SLQ_FLOATINGBASESLQ_H_ */
diff --git a/ct_rbd/include/ct/rbd/state/JointAcceleration.h b/ct_rbd/include/ct/rbd/state/JointAcceleration.h
new file mode 100644
index 0000000..ef1224e
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/state/JointAcceleration.h
@@ -0,0 +1,60 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace rbd {
+namespace tpl {
+
+/*!
+ * \ingroup State
+ *
+ * \brief joint acceleration
+ */
+template <size_t NJOINTS, typename SCALAR>
+class JointAcceleration
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ enum NDOFS
+ {
+ NDOFS = NJOINTS
+ };
+
+ JointAcceleration() { setZero(); }
+ JointAcceleration(const Eigen::Matrix<SCALAR, NJOINTS, 1>& acceleration) : acceleration_(acceleration) {}
+ /// @brief get number of degrees of freedom
+ size_t getSize() { return NJOINTS; }
+ /// @brief get i-th joint acceleration
+ SCALAR& operator()(size_t i) { return acceleration_(i); }
+ /// @brief get joint acceleration
+ Eigen::Matrix<SCALAR, NJOINTS, 1>& getAcceleration() { return acceleration_; }
+ /// @brief get constant joint acceleration
+ const Eigen::Matrix<SCALAR, NJOINTS, 1>& getAcceleration() const { return acceleration_; }
+ /// @brief set joint acceleration
+ void setAcceleration(const Eigen::Matrix<SCALAR, NJOINTS, 1>& acceleration) { acceleration_ = acceleration; }
+ /// @brief set joint acceleration to zero
+ void setZero() { acceleration_.setZero(); }
+ static JointAcceleration<NJOINTS, SCALAR> Zero()
+ {
+ JointAcceleration<NJOINTS, SCALAR> jstate;
+ jstate.setZero();
+ return jstate;
+ }
+
+protected:
+ Eigen::Matrix<SCALAR, NJOINTS, 1> acceleration_;
+};
+
+} // namespace tpl
+
+template <size_t NJOINTS>
+using JointAcceleration = tpl::JointAcceleration<NJOINTS, double>;
+
+} // namespace rbd
+} // namespace ct
diff --git a/ct_rbd/include/ct/rbd/state/JointState.h b/ct_rbd/include/ct/rbd/state/JointState.h
new file mode 100644
index 0000000..7d61c1a
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/state/JointState.h
@@ -0,0 +1,143 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/core/core.h>
+
+namespace ct {
+namespace rbd {
+namespace tpl {
+
+/**
+ * @class JointState
+ *
+ * \ingroup State
+ *
+ * @brief joint state and joint velocity
+ */
+template <size_t NJOINTS, typename SCALAR = double>
+class JointState
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ enum NDOFS
+ {
+ NDOFS = NJOINTS
+ };
+
+ typedef Eigen::Matrix<SCALAR, 2 * NJOINTS, 1> joint_state_vector_t;
+ typedef Eigen::Matrix<SCALAR, NJOINTS, 1> Position;
+ typedef Eigen::Matrix<SCALAR, NJOINTS, 1> Velocity;
+
+ typedef Eigen::VectorBlock<joint_state_vector_t, NJOINTS> JointPositionBlock;
+ typedef Eigen::VectorBlock<const joint_state_vector_t, NJOINTS> JointPositionBlockConst;
+
+ JointState() { setZero(); }
+ JointState(const joint_state_vector_t& state) : state_(state) {}
+ inline bool operator==(const JointState& rhs) const { return (this->state_ == rhs.state_); }
+ inline bool operator!=(const JointState<NJOINTS, SCALAR>& rhs) const { return (this->state_ != rhs.state_); }
+ inline JointState<NJOINTS, SCALAR> operator+(const JointState<NJOINTS, SCALAR>& rhs) const
+ {
+ return JointState<NJOINTS, SCALAR>(this->state_ + rhs.state_);
+ }
+ inline JointState<NJOINTS, SCALAR> operator-(const JointState<NJOINTS, SCALAR>& rhs) const
+ {
+ return JointState<NJOINTS, SCALAR>(this->state_ - rhs.state_);
+ }
+
+ inline JointState<NJOINTS, SCALAR> operator*(const SCALAR& scalar) const
+ {
+ return JointState<NJOINTS, SCALAR>(this->state_ * scalar);
+ }
+ inline JointState<NJOINTS, SCALAR> operator/(const SCALAR& scalar) const
+ {
+ return JointState<NJOINTS, SCALAR>(this->state_ / scalar);
+ }
+
+ inline bool operator<(const JointState<NJOINTS, SCALAR>& rhs) const
+ {
+ return (this->array() < rhs.array()).isConstant(1);
+ }
+
+ inline bool operator>(const JointState<NJOINTS, SCALAR>& rhs) const
+ {
+ return (this->array() > rhs.array()).isConstant(1);
+ }
+
+ bool isApprox(const JointState<NJOINTS, SCALAR>& rhs, const SCALAR& tol = 1e-10)
+ {
+ return state_.isApprox(rhs.state_, tol);
+ }
+
+ /// @brief get joint state
+ JointPositionBlock getPositions() { return state_.template head<NJOINTS>(); }
+ /// @brief get constant joint state
+ const JointPositionBlockConst getPositions() const { return state_.template head<NJOINTS>(); }
+ SCALAR& getPosition(size_t i)
+ {
+ assert(i < NJOINTS && "Invalid joint index");
+ return state_(i);
+ }
+ const SCALAR& getPosition(size_t i) const
+ {
+ assert(i < NJOINTS && "Invalid joint index");
+ return state_(i);
+ }
+ /// @brief check joint position limits
+ template <typename T>
+ bool checkPositionLimits(T lowerLimit, T upperLimit)
+ {
+ assert(lowerLimit.size() == NJOINTS && upperLimit.size() == NJOINTS && "Wrong limit dimensions");
+ for (size_t i = 0; i < NJOINTS; ++i)
+ if (getPosition(i) < lowerLimit[i] || getPosition(i) > upperLimit[i])
+ return false;
+ return true;
+ }
+
+ /// @brief get joint velocity
+ JointPositionBlock getVelocities() { return state_.template tail<NJOINTS>(); }
+ /// @brief get constant joint velocity
+ const JointPositionBlockConst getVelocities() const { return state_.template tail<NJOINTS>(); }
+ SCALAR& getVelocity(size_t i)
+ {
+ assert(i < NJOINTS && "Invalid joint index");
+ return state_(NJOINTS + i);
+ }
+ const SCALAR& getVelocity(size_t i) const
+ {
+ assert(i < NJOINTS && "Invalid joint index");
+ return state_(NJOINTS + i);
+ }
+ /// @brief check joint velocity limits
+ template <typename T>
+ bool checkVelocityLimits(T limit)
+ {
+ assert(limit.size() == NJOINTS && "Wrong limit dimension");
+ for (size_t i = 0; i < NJOINTS; ++i)
+ if (abs(getVelocity(i)) > limit[i])
+ return false;
+ return true;
+ }
+
+
+ joint_state_vector_t& toImplementation() { return state_; }
+ const joint_state_vector_t& toImplementation() const { return state_; }
+ /// @brief set states to zero
+ void setZero() { state_.setZero(); }
+ void setRandom() { state_.setRandom(); }
+protected:
+ joint_state_vector_t state_;
+};
+
+} // namespace tpl
+
+template <size_t NJOINTS>
+using JointState = tpl::JointState<NJOINTS, double>;
+
+} // namespace rbd
+} // namespace ct
diff --git a/ct_rbd/include/ct/rbd/state/RBDAcceleration.h b/ct_rbd/include/ct/rbd/state/RBDAcceleration.h
new file mode 100644
index 0000000..7a102a9
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/state/RBDAcceleration.h
@@ -0,0 +1,124 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#ifndef RBDACCELERATION_H_
+#define RBDACCELERATION_H_
+
+#include "JointAcceleration.h"
+#include "RBDState.h"
+#include "RigidBodyAcceleration.h"
+
+namespace ct {
+namespace rbd {
+namespace tpl {
+
+/**
+ * @class RBDAcceleration
+ *
+ * \ingroup State
+ *
+ * @brief joint acceleration and base acceleration
+ */
+template <size_t NJOINTS, typename SCALAR>
+class RBDAcceleration
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ enum DIMS
+ {
+ NDOF = NJOINTS + 6,
+ NSTATE = 2 * NDOF ///< position/velocity of (joints + base)
+
+ };
+
+ typedef Eigen::Matrix<SCALAR, NDOF, 1> coordinate_vector_t;
+
+ typedef RigidBodyAcceleration<SCALAR> RigidBodyAcceleration_t;
+
+
+ RBDAcceleration() { setZero(); }
+ RBDAcceleration(const RigidBodyAcceleration_t& baseStateDerivative,
+ const JointAcceleration<NJOINTS, SCALAR>& jointStateDerivative)
+ : baseStateDerivative_(baseStateDerivative), jointStateDerivative_(jointStateDerivative)
+ {
+ }
+
+ /// @brief get base acceleration
+ RigidBodyAcceleration_t& base() { return baseStateDerivative_; }
+ /// @brief get constant base acceleration
+ const RigidBodyAcceleration_t& base() const { return baseStateDerivative_; }
+ /// @brief get joint acceleration
+ JointAcceleration<NJOINTS, SCALAR>& joints() { return jointStateDerivative_; }
+ /// @brief get constant joint acceleration
+ const JointAcceleration<NJOINTS, SCALAR>& joints() const { return jointStateDerivative_; }
+ typename RBDState<NJOINTS, SCALAR>::state_vector_quat_t toStateUpdateVectorQuaternion(
+ const RBDState<NJOINTS, SCALAR>& state) const
+ {
+ typename RBDState<NJOINTS, SCALAR>::state_vector_quat_t stateDerivative;
+
+ kindr::RotationQuaternionDiff<SCALAR> rotationQuaternionDiff(
+ state.basePose().getRotationQuaternion(), state.baseLocalAngularVelocity());
+
+ stateDerivative << rotationQuaternionDiff.w(), rotationQuaternionDiff.x(), rotationQuaternionDiff.y(),
+ rotationQuaternionDiff.z(), state.base().computeTranslationalVelocityW().toImplementation(),
+ state.joints().getVelocities(), base().getAngularAcceleration().toImplementation(),
+ base().getTranslationalAcceleration().toImplementation(), joints().getAcceleration();
+
+ return stateDerivative;
+ }
+
+ typename RBDState<NJOINTS, SCALAR>::state_vector_euler_t toStateUpdateVectorEulerXyz(
+ const RBDState<NJOINTS, SCALAR>& state) const
+ {
+ typename RBDState<NJOINTS, SCALAR>::state_vector_euler_t stateDerivative;
+
+ kindr::EulerAnglesXyzDiff<SCALAR> eulerAnglesXyzDiff(
+ state.basePose().getEulerAnglesXyz(), state.baseLocalAngularVelocity());
+
+ stateDerivative << eulerAnglesXyzDiff.toImplementation(),
+ state.base().computeTranslationalVelocityW().toImplementation(), state.joints().getVelocities(),
+ base().getAngularAcceleration().toImplementation(),
+ base().getTranslationalAcceleration().toImplementation(), joints().getAcceleration();
+
+ return stateDerivative;
+ }
+
+ coordinate_vector_t toCoordinateAcceleration() const
+ {
+ coordinate_vector_t ddq;
+ ddq << base().getAngularAcceleration().toImplementation(),
+ base().getTranslationalAcceleration().toImplementation(), joints().getAcceleration();
+ return ddq;
+ }
+
+ void setZero()
+ {
+ baseStateDerivative_.setZero();
+ jointStateDerivative_.setZero();
+ }
+
+ static RBDAcceleration<NJOINTS, SCALAR> Zero()
+ {
+ return RBDAcceleration<NJOINTS, SCALAR>(
+ RigidBodyAcceleration_t::Zero(), JointAcceleration<NJOINTS, SCALAR>::Zero());
+ }
+
+protected:
+ RigidBodyAcceleration_t baseStateDerivative_;
+ JointAcceleration<NJOINTS, SCALAR> jointStateDerivative_;
+};
+
+} // namespace tpl
+
+template <size_t NJOINTS>
+using RBDAcceleration = tpl::RBDAcceleration<NJOINTS, double>;
+
+} // namespace rbd
+} // namespace ct
+
+
+#endif /* RBDACCELERATION_H_ */
diff --git a/ct_rbd/include/ct/rbd/state/RBDState.h b/ct_rbd/include/ct/rbd/state/RBDState.h
new file mode 100644
index 0000000..412ea39
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/state/RBDState.h
@@ -0,0 +1,244 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "JointState.h"
+#include "RigidBodyState.h"
+
+namespace ct {
+namespace rbd {
+namespace tpl {
+
+/** \defgroup State State
+ * \brief Different state types for Rigid Bodies, Robots etc.
+ */
+
+
+/*!
+ *
+ * \ingroup State
+ *
+ * @brief joint states and base states
+ */
+
+template <size_t NJOINTS, typename SCALAR>
+class RBDState
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ enum DIMS
+ {
+ NDOF = NJOINTS + 6,
+ NSTATE = 2 * (NJOINTS + 6) ///< position/velocity of (joints + base)
+
+ };
+
+ typedef Eigen::Matrix<SCALAR, NSTATE + 1, 1> state_vector_quat_t;
+ typedef Eigen::Matrix<SCALAR, NSTATE, 1> state_vector_euler_t;
+ typedef Eigen::Matrix<SCALAR, NDOF, 1> coordinate_vector_t;
+
+ RBDState(typename RigidBodyPose<SCALAR>::STORAGE_TYPE storage = RigidBodyPose<SCALAR>::EULER) : baseState_(storage)
+ {
+ baseState_.setIdentity();
+ jointState_.setZero();
+ }
+
+ RBDState(const RBDState& other) : baseState_(other.baseState_), jointState_(other.jointState_) {}
+ RBDState(const RigidBodyState<SCALAR>& baseState, const JointState<NJOINTS, SCALAR>& jointState)
+ : baseState_(baseState), jointState_(jointState)
+ {
+ }
+
+ virtual ~RBDState() {}
+ bool operator!=(const RBDState& other) const { return (base() != other.base() || joints() != other.joints()); }
+ bool isApprox(const RBDState& rhs, const double& tol = 1e-10)
+ {
+ return base().isApprox(rhs.base(), tol) && joints().isApprox(rhs.joints(), tol);
+ }
+
+
+ /// @brief get base states
+ RigidBodyState<SCALAR>& base() { return baseState_; }
+ /// @brief get constant base states
+ const RigidBodyState<SCALAR>& base() const { return baseState_; }
+ /// @brief get base pose
+ RigidBodyPose<SCALAR>& basePose() { return base().pose(); }
+ /// @brief get constant base states
+ const RigidBodyPose<SCALAR>& basePose() const { return base().pose(); }
+ /// @brief get base velocities
+ RigidBodyVelocities<SCALAR>& baseVelocities() { return base().velocities(); }
+ /// @brief get constant base velocities
+ const RigidBodyVelocities<SCALAR>& baseVelocities() const { return base().velocities(); }
+ /// @brief get base local angular velocity
+ kindr::LocalAngularVelocity<SCALAR>& baseLocalAngularVelocity()
+ {
+ return base().velocities().getRotationalVelocity();
+ }
+ /// @brief get constant base local angular velocity
+ const kindr::LocalAngularVelocity<SCALAR>& baseLocalAngularVelocity() const
+ {
+ return base().velocities().getRotationalVelocity();
+ }
+
+ /// @brief get base linear velocity
+ kindr::Velocity<SCALAR, 3>& baseLinearVelocity() { return base().velocities().getTranslationalVelocity(); }
+ /// @brief get constant base local angular velocity
+ const kindr::Velocity<SCALAR, 3>& baseLinearVelocity() const
+ {
+ return base().velocities().getTranslationalVelocity();
+ }
+
+ /// @brief get joint states
+ JointState<NJOINTS, SCALAR>& joints() { return jointState_; }
+ /// @brief get constant joint states
+ const JointState<NJOINTS, SCALAR>& joints() const { return jointState_; }
+ //BLOCKS ARE ALREADY REFERENCES, DO NOT MAKE THESE RETURN REFERENCES TO BLOCKS
+ /// @brief get joint positions
+ typename JointState<NJOINTS, SCALAR>::JointPositionBlock jointPositions() { return joints().getPositions(); }
+ /// @brief get constant joint positions
+ const typename JointState<NJOINTS, SCALAR>::JointPositionBlockConst jointPositions() const
+ {
+ return joints().getPositions();
+ }
+
+ //BLOCKS ARE ALREADY REFERENCES, DO NOT MAKE THESE RETURN REFERENCES TO BLOCKS
+ /// @brief get joint velocities
+ typename JointState<NJOINTS, SCALAR>::JointPositionBlock jointVelocities() { return joints().getVelocities(); }
+ /// @brief get constant joint velocities
+ const typename JointState<NJOINTS, SCALAR>::JointPositionBlockConst jointVelocities() const
+ {
+ return joints().getVelocities();
+ }
+
+ state_vector_quat_t toStateVectorQuaternion() const
+ {
+ state_vector_quat_t state;
+
+ state << base().pose().getRotationQuaternion().w(), base().pose().getRotationQuaternion().x(),
+ base().pose().getRotationQuaternion().y(), base().pose().getRotationQuaternion().z(),
+ base().pose().position().toImplementation(), joints().getPositions(),
+ base().velocities().getRotationalVelocity().toImplementation(),
+ base().velocities().getTranslationalVelocity().toImplementation(), joints().getVelocities();
+
+ return state;
+ }
+
+ coordinate_vector_t toCoordinatePositionUnique() const
+ {
+ coordinate_vector_t q;
+ q << base().pose().getEulerAnglesXyz().getUnique().toImplementation(),
+ base().pose().position().toImplementation(), joints().getPositions();
+ return q;
+ }
+
+ coordinate_vector_t toCoordinatePosition() const
+ {
+ coordinate_vector_t q;
+ q << base().pose().getEulerAnglesXyz().toImplementation(), base().pose().position().toImplementation(),
+ joints().getPositions();
+ return q;
+ }
+
+ coordinate_vector_t toCoordinateVelocity() const
+ {
+ coordinate_vector_t dq;
+ dq << base().velocities().getRotationalVelocity().toImplementation(),
+ base().velocities().getTranslationalVelocity().toImplementation(), joints().getVelocities();
+ return dq;
+ }
+
+ state_vector_euler_t toStateVectorEulerXyzUnique() const
+ {
+ state_vector_euler_t state;
+
+ state << toCoordinatePositionUnique(), toCoordinateVelocity();
+
+ return state;
+ }
+
+ state_vector_euler_t toStateVectorEulerXyz() const
+ {
+ state_vector_euler_t state;
+
+ state << toCoordinatePosition(), toCoordinateVelocity();
+
+ return state;
+ }
+
+ void fromStateVectorRaw(const state_vector_quat_t& state) { fromStateVectorQuaternion(state); }
+ void fromStateVectorRaw(const state_vector_euler_t& state) { fromStateVectorEulerXyz(state); }
+ void fromStateVectorQuaternion(const state_vector_quat_t& state)
+ {
+ try
+ {
+ base().pose().setFromRotationQuaternion(kindr::RotationQuaternionD(state.template head<4>()));
+ base().pose().position().toImplementation() = state.template segment<3>(4);
+ joints().getPositions() = state.template segment<NJOINTS>(7);
+ base().velocities().getRotationalVelocity().toImplementation() = state.template segment<3>(7 + NJOINTS);
+ base().velocities().getTranslationalVelocity().toImplementation() = state.template segment<3>(10 + NJOINTS);
+ joints().getVelocities() = state.template tail<NJOINTS>();
+ } catch (std::exception& e)
+ {
+ std::cout << "Conversion from State Vector to KindrTypes failed." << std::endl;
+ std::cout << "Error: " << e.what() << std::endl;
+ std::cout << "State is: " << state.transpose() << std::endl;
+ throw std::runtime_error("Conversion from State Vector to KindrTypes failed.");
+ }
+ }
+
+ void fromStateVectorEulerXyz(const state_vector_euler_t& state)
+ {
+ try
+ {
+ base().pose().setFromEulerAnglesXyz(state.template head<3>());
+ base().pose().position().toImplementation() = state.template segment<3>(3);
+ joints().getPositions() = state.template segment<NJOINTS>(6);
+ base().velocities().getRotationalVelocity().toImplementation() = state.template segment<3>(6 + NJOINTS);
+ base().velocities().getTranslationalVelocity().toImplementation() = state.template segment<3>(9 + NJOINTS);
+ joints().getVelocities() = state.template tail<NJOINTS>();
+ } catch (std::exception& e)
+ {
+ std::cout << "Conversion from State Vector to KindrTypes failed." << std::endl;
+ std::cout << "Error: " << e.what() << std::endl;
+ std::cout << "State is: " << state.transpose() << std::endl;
+ throw std::runtime_error("Conversion from State Vector to KindrTypes failed.");
+ }
+ }
+
+ virtual void setDefault()
+ {
+ baseState_.setIdentity();
+ jointState_.setZero();
+ }
+
+ void setZero()
+ {
+ baseState_.setIdentity();
+ jointState_.setZero();
+ }
+
+ void setRandom()
+ {
+ baseState_.setRandom();
+ jointState_.setRandom();
+ }
+
+
+protected:
+ RigidBodyState<SCALAR> baseState_;
+ JointState<NJOINTS, SCALAR> jointState_;
+};
+
+
+} // namespace tpl
+
+template <size_t NJOINTS>
+using RBDState = tpl::RBDState<NJOINTS, double>;
+
+} // namespace rbd
+} // namespace ct
diff --git a/ct_rbd/include/ct/rbd/state/RigidBodyAcceleration.h b/ct_rbd/include/ct/rbd/state/RigidBodyAcceleration.h
new file mode 100644
index 0000000..67ff95e
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/state/RigidBodyAcceleration.h
@@ -0,0 +1,94 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace rbd {
+namespace tpl {
+
+/**
+ * @class RigidBodyAcceleration
+ *
+ * \ingroup State
+ *
+ * @brief acceleration of a rigid body
+ */
+template <typename SCALAR>
+class RigidBodyAcceleration
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef kindr::Acceleration<SCALAR, 3> LinearAcceleration;
+ typedef kindr::AngularAcceleration<SCALAR, 3> AngularAcceleration;
+
+ RigidBodyAcceleration() { setZero(); }
+ RigidBodyAcceleration(LinearAcceleration& transAcceleration, AngularAcceleration& anglAcceleration)
+ : anglAcceleration_(anglAcceleration), transAcceleration_(transAcceleration)
+ {
+ }
+
+ RigidBodyAcceleration(const Eigen::Matrix<SCALAR, 6, 1>& in) { fromVector6d(in); }
+ void fromVector6d(const Eigen::Matrix<SCALAR, 6, 1>& in)
+ {
+ anglAcceleration_ << in.template head<3>();
+ transAcceleration_ << in.template tail<3>();
+ }
+
+ const Eigen::Matrix<SCALAR, 6, 1> getVector6d() const
+ {
+ Eigen::Matrix<SCALAR, 6, 1> vector6;
+ vector6 << anglAcceleration_.toImplementation(), transAcceleration_.toImplementation();
+ return vector6;
+ }
+
+ /// @brief get translatory acceleration
+ LinearAcceleration& getTranslationalAcceleration() { return transAcceleration_; }
+ /// @brief get constant translatory acceleration
+ const LinearAcceleration& getTranslationalAcceleration() const { return transAcceleration_; }
+ /// @brief get angular acceleration
+ AngularAcceleration& getAngularAcceleration() { return anglAcceleration_; }
+ /// @brief get constant angular acceleration
+ const AngularAcceleration& getAngularAcceleration() const { return anglAcceleration_; }
+ /// @brief get accelerations as a "Twist"
+ Eigen::Matrix<SCALAR, 6, 1> accelerations()
+ {
+ Eigen::Matrix<SCALAR, 6, 1> out;
+ out << anglAcceleration_.toImplementation(), transAcceleration_.toImplementation();
+ return out;
+ }
+
+ /// @brief set translatory acceleration
+ void setTranslatoryAcceleration(LinearAcceleration& transAcceleration) { transAcceleration_ = transAcceleration; }
+ /// @brief set angular acceleration
+ void setAngularAcceleration(AngularAcceleration& anglAcceleration) { anglAcceleration_ = anglAcceleration; }
+ /// @brief set acceleration to zero
+ void setZero()
+ {
+ // cannot use .setZero() from members here due to codegen
+ anglAcceleration_ = AngularAcceleration(SCALAR(0.0), SCALAR(0.0), SCALAR(0.0));
+ transAcceleration_ = LinearAcceleration(SCALAR(0.0), SCALAR(0.0), SCALAR(0.0));
+ }
+
+ static RigidBodyAcceleration Zero()
+ {
+ RigidBodyAcceleration state;
+ state.setZero();
+ return state;
+ }
+
+protected:
+ AngularAcceleration anglAcceleration_;
+ LinearAcceleration transAcceleration_;
+};
+
+} // namespace tpl
+
+typedef tpl::RigidBodyAcceleration<double> RigidBodyAcceleration;
+
+} // namespace rbd
+} // namespace ct
diff --git a/ct_rbd/include/ct/rbd/state/RigidBodyPose.h b/ct_rbd/include/ct/rbd/state/RigidBodyPose.h
new file mode 100644
index 0000000..f17a03c
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/state/RigidBodyPose.h
@@ -0,0 +1,420 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <kindr/Core>
+
+namespace ct {
+namespace rbd {
+namespace tpl {
+
+/**
+ * \brief Pose of a single rigid body
+ *
+ * \ingroup State
+ *
+ * This implements the pose of a single rigid body. Internally, the pose is stored as
+ * either a quaternion (default) or Euler angles. Hence, the user is encouraged to use
+ * rotateBaseToInertia() or rotateInertiaToBase() instead of directly working with either
+ * representation. This will prevent unnecessary conversions.
+ */
+template <typename SCALAR = double>
+class RigidBodyPose
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ /*!
+ * \brief the orientation can either be stored as EulerAngles or as quaternion.
+ * \todo reduce to quaternion only
+ */
+ enum STORAGE_TYPE
+ {
+ QUAT = 0,
+ EULER = 1
+ };
+
+ using HomogeneousTransform = kindr::HomogeneousTransformationPosition3RotationQuaternion<SCALAR>;
+ using Matrix4Tpl = Eigen::Matrix<SCALAR, 4, 4>;
+ using Matrix3Tpl = Eigen::Matrix<SCALAR, 3, 3>;
+ using Position3Tpl = kindr::Position<SCALAR, 3>;
+ using Vector3Tpl = Eigen::Matrix<SCALAR, 3, 1>;
+
+ //! default construct a RigidBodyPose
+ RigidBodyPose(STORAGE_TYPE storage = EULER)
+ : storage_(storage),
+ quat_(SCALAR(1.0), SCALAR(0.0), SCALAR(0.0), SCALAR(0.0)), // for CppAD cg compatibility
+ euler_(SCALAR(0.0), SCALAR(0.0), SCALAR(0.0))
+ {
+ }
+
+ //! construct a RigidBodyPose from Euler Angles and a position vector
+ RigidBodyPose(const kindr::EulerAnglesXyz<SCALAR>& orientationEulerXyz,
+ const Position3Tpl& position,
+ STORAGE_TYPE storage = EULER)
+ : storage_(storage),
+ quat_(SCALAR(1.0), SCALAR(0.0), SCALAR(0.0), SCALAR(0.0)), // for CppAD cg compatibility
+ euler_(SCALAR(0.0), SCALAR(0.0), SCALAR(0.0)),
+ position_(position)
+ {
+ setFromEulerAnglesXyz(orientationEulerXyz);
+ }
+
+ //! construct a RigidBodyPose from a rotation quaternion and a position vector
+ RigidBodyPose(const kindr::RotationQuaternion<SCALAR>& orientationQuat,
+ const Position3Tpl& position,
+ STORAGE_TYPE storage = EULER)
+ : storage_(storage),
+ quat_(SCALAR(1.0), SCALAR(0.0), SCALAR(0.0), SCALAR(0.0)), // for CppAD cg compatibility
+ euler_(SCALAR(0.0), SCALAR(0.0), SCALAR(0.0)),
+ position_(position)
+ {
+ setFromRotationQuaternion(orientationQuat);
+ }
+
+ //! construct a RigidBodyPose from a homogeneous transformation matrix
+ RigidBodyPose(const Matrix4Tpl& homTransform, STORAGE_TYPE storage = EULER)
+ : storage_(storage),
+ quat_(SCALAR(1.0), SCALAR(0.0), SCALAR(0.0), SCALAR(0.0)), // for CppAD cg compatibility
+ euler_(SCALAR(0.0), SCALAR(0.0), SCALAR(0.0)),
+ position_(homTransform.template topRightCorner<3, 1>())
+ {
+ kindr::RotationMatrix<SCALAR> rotMat(homTransform.template topLeftCorner<3, 3>());
+ setFromRotationMatrix(rotMat);
+ }
+
+ //! copy-constructor for a RigidBodyPose
+ RigidBodyPose(const RigidBodyPose<SCALAR>& arg)
+ : storage_(arg.storage_), quat_(arg.quat_), euler_(arg.euler_), position_(arg.position_)
+ {
+ }
+
+ //RigidBodyPose(const Eigen::Vector3d& orientationEulerXyz, const Eigen::Vector3d& position, STORAGE_TYPE storage = QUAT);
+ //RigidBodyPose(const Eigen::Quaterniond& orientationQuat, const Eigen::Vector3d& position, STORAGE_TYPE storage = QUAT);
+
+ //! destructor for a rigid body pose
+ ~RigidBodyPose(){};
+
+ inline bool isNear(const RigidBodyPose& rhs, const double& tol = 1e-10) const
+ {
+ return getRotationQuaternion().isNear(rhs.getRotationQuaternion(), tol) &&
+ position().toImplementation().isApprox(rhs.position().toImplementation(), tol);
+ }
+
+ /**
+ * \brief This method returns the Euler angles rotation (X,Y,Z / roll,pitch,yaw).
+ */
+ kindr::EulerAnglesXyz<SCALAR> getEulerAnglesXyz() const
+ {
+ if (storedAsEuler())
+ {
+ return euler_;
+ }
+ else
+ {
+ return kindr::EulerAnglesXyz<SCALAR>(quat_);
+ }
+ }
+
+ /**
+ * \brief This method returns the quaternion rotation.
+ */
+ kindr::RotationQuaternion<SCALAR> getRotationQuaternion() const
+ {
+ if (storedAsEuler())
+ {
+ return kindr::RotationQuaternion<SCALAR>(euler_);
+ }
+ else
+ {
+ return quat_;
+ }
+ }
+
+ /**
+ * \brief This method returns the rotation matrix from base to inertia frame. This method returns
+ * \f$ _iR_b \f$ which is the following rotation matrix \f$ _iv = {_iR_b} {_bv} \f$.
+ */
+ kindr::RotationMatrix<SCALAR> getRotationMatrix() const
+ {
+ if (storedAsEuler())
+ {
+ Eigen::AngleAxis<SCALAR> rollAngle(euler_.toImplementation()(0), Eigen::Matrix<SCALAR, 3, 1>::UnitX());
+ Eigen::AngleAxis<SCALAR> pitchAngle(euler_.toImplementation()(1), Eigen::Matrix<SCALAR, 3, 1>::UnitY());
+ Eigen::AngleAxis<SCALAR> yawAngle(euler_.toImplementation()(2), Eigen::Matrix<SCALAR, 3, 1>::UnitZ());
+ Eigen::Quaternion<SCALAR> q = rollAngle * pitchAngle * yawAngle;
+ return kindr::RotationMatrix<SCALAR>(q.matrix());
+
+ // return kindr::RotationMatrix<SCALAR>(euler_); // this is not JIT compatible as it uses a unit quat temporarily
+ }
+ else
+ {
+ return kindr::RotationMatrix<SCALAR>(quat_);
+ }
+ }
+
+ /**
+ * \brief This method sets the Euler angles rotation (X,Y,Z / roll,pitch,yaw) from a kindr type.
+ */
+ void setFromEulerAnglesXyz(const kindr::EulerAnglesXyz<SCALAR>& eulerAngles)
+ {
+ if (storedAsEuler())
+ {
+ euler_.toImplementation() = eulerAngles.toImplementation();
+ }
+ else
+ {
+ quat_ = eulerAngles;
+ }
+ }
+
+ /**
+ * \brief This method sets the Euler angles rotation (X,Y,Z / roll,pitch,yaw) from a kindr type.
+ */
+ void setFromEulerAnglesXyz(const Vector3Tpl& eulerAngles)
+ {
+ if (storedAsEuler())
+ {
+ euler_.toImplementation() = eulerAngles;
+ }
+ else
+ {
+ quat_ = kindr::EulerAnglesXyz<SCALAR>(eulerAngles);
+ }
+ }
+
+ /**
+ * \brief This method sets the quaternion angles rotation (X,Y,Z / roll,pitch,yaw) from a kindr quaternion type.
+ */
+ void setFromRotationQuaternion(const kindr::RotationQuaternion<SCALAR>& quat)
+ {
+ if (storedAsEuler())
+ {
+ euler_ = quat;
+ }
+ else
+ {
+ quat_ = quat;
+ }
+ }
+
+ /**
+ * \brief This method sets the quaternion angles rotation (X,Y,Z / roll,pitch,yaw) from an Eigen quaternion
+ */
+ void setFromRotationQuaternion(const Eigen::Quaterniond& quat)
+ {
+ if (storedAsEuler())
+ {
+ euler_ = kindr::EulerAnglesXyz<SCALAR>(quat);
+ }
+ else
+ {
+ quat_ = kindr::RotationQuaternion<SCALAR>(quat);
+ }
+ }
+
+ /**
+ * \brief This method sets the quaternion angles rotation from a kindr rotation matrix
+ */
+ void setFromRotationMatrix(const kindr::RotationMatrix<SCALAR>& rotMat)
+ {
+ if (storedAsEuler())
+ {
+ euler_ = kindr::EulerAnglesXyz<SCALAR>(rotMat);
+ }
+ else
+ {
+ quat_ = kindr::RotationQuaternion<SCALAR>(rotMat);
+ }
+ }
+
+ /**
+ * \brief This method returns the position of the Base frame in the inertia frame.
+ */
+
+ const Position3Tpl& position() const { return position_; };
+ /**
+ * \brief This method returns the position of the Base frame in the inertia frame.
+ */
+
+ Position3Tpl& position() { return position_; };
+ /**
+ * \brief This method returns the Base frame in a custom Frame specified in the Inertia Frame.
+ */
+ RigidBodyPose<SCALAR> inReferenceFrame(const RigidBodyPose<SCALAR>& ref_frame) const
+ {
+ if (ref_frame.storedAsEuler() && storedAsEuler())
+ {
+ return RigidBodyPose<SCALAR>(ref_frame.getEulerAnglesXyz().inverted() * euler_,
+ ref_frame.getEulerAnglesXyz().inverseRotate(position() - ref_frame.position()));
+ }
+ else if (ref_frame.storedAsEuler() && !storedAsEuler())
+ {
+ return RigidBodyPose<SCALAR>(ref_frame.getEulerAnglesXyz().inverted() * quat_,
+ ref_frame.getEulerAnglesXyz().inverseRotate(position() - ref_frame.position()));
+ }
+ else if (!ref_frame.storedAsEuler() && storedAsEuler())
+ {
+ return RigidBodyPose<SCALAR>(ref_frame.getRotationQuaternion().inverted() * euler_,
+ ref_frame.getRotationQuaternion().inverseRotate(position() - ref_frame.position()));
+ }
+ else
+ {
+ return RigidBodyPose<SCALAR>(ref_frame.getRotationQuaternion().inverted() * quat_,
+ ref_frame.getRotationQuaternion().inverseRotate(position() - ref_frame.position()));
+ }
+ };
+
+ /**
+ * \brief This methods rotates a 3D vector expressed in Base frame to Inertia Frame.
+ */
+ template <class Vector3s>
+ Vector3s rotateBaseToInertia(const Vector3s& vector) const
+ {
+ Vector3s out;
+
+ if (storedAsEuler())
+ {
+ kindr::RotationMatrix<SCALAR> rotationMatrix = getRotationMatrix();
+
+ // incredibly ugly hack to get along with different types
+ Eigen::Matrix<SCALAR, 3, 1> vec_temp;
+ vec_temp << vector(0), vector(1), vector(2);
+
+ Eigen::Matrix<SCALAR, 3, 1> result = rotationMatrix.toImplementation() * vec_temp;
+ out = (Vector3s)result;
+
+ // return euler_.rotate(vector); // temporarily replaced -- the kindr rotate() method is not auto-diffable
+ }
+ else
+ {
+ out = quat_.rotate(vector);
+ }
+
+ return out;
+ };
+
+
+ /**
+ * \brief This methods rotates a 3D matrix expressed in Base frame to Inertia Frame.
+ */
+ Matrix3Tpl rotateBaseToInertiaMat(const Matrix3Tpl& mat) const
+ {
+ kindr::RotationMatrix<SCALAR> rotMat = getRotationMatrix();
+ return rotMat.toImplementation() * mat;
+ }
+
+
+ /**
+ * \brief This methods rotates a quaternion expressed in Base frame to Inertia Frame.
+ */
+ kindr::RotationQuaternion<SCALAR> rotateBaseToInertiaQuaternion(const kindr::RotationQuaternion<SCALAR>& q) const
+ {
+ kindr::RotationQuaternion<SCALAR> result;
+
+ if (storedAsEuler())
+ result = kindr::RotationQuaternion<SCALAR>(euler_) * q;
+ else
+ result = quat_ * q;
+
+ return result;
+ }
+
+ /**
+ * \brief This methods rotates a 3D vector expressed in Inertia frame to Base Frame.
+ */
+ template <class Vector3s>
+ Vector3s rotateInertiaToBase(const Vector3s& vector) const
+ {
+ // more efficient than inverseRotate and (more importantly) compatible with auto-diff
+ // https://github.com/ethz-asl/kindr/issues/84
+ // return Vector3d::Zero();
+ // return kindr::RotationMatrix<SCALAR>(euler_).transposed().rotate(vector);
+ if (storedAsEuler())
+ {
+ return kindr::RotationMatrix<SCALAR>(euler_).transposed().rotate(vector);
+ }
+ else
+ {
+ return quat_.inverseRotate(vector);
+ }
+ };
+
+ /**
+ * \brief This methods returns the Homogeneous transform from the Base frame to the inertia frame.
+ */
+ HomogeneousTransform getHomogeneousTransform() const
+ {
+ throw std::runtime_error("get homogeneous transform not implemented");
+ return HomogeneousTransform();
+ };
+
+ /**
+ * Returns gravity vector in world (0.0, 0.0, -9.81)
+ * @return
+ */
+ static Vector3Tpl gravity3DW(SCALAR g = SCALAR(-9.81)) { return Vector3Tpl(SCALAR(0.0), SCALAR(0.0), g); }
+ /**
+ * \brief This methods returns the 3D gravity vector expressed in the Base frame.
+ */
+ Vector3Tpl computeGravityB(const Vector3Tpl& gravityW = gravity3DW()) const
+ {
+ return rotateInertiaToBase(gravityW);
+ }
+
+ /**
+ * \brief This methods returns the 6D gravity vector expressed in the Base frame.
+ */
+ Eigen::Matrix<SCALAR, 6, 1> computeGravityB6D(const Vector3Tpl& gravityW = gravity3DW()) const
+ {
+ Eigen::Matrix<SCALAR, 6, 1> gravityWout(Eigen::Matrix<SCALAR, 6, 1>::Zero());
+ gravityWout.template tail<3>() = rotateInertiaToBase(gravityW);
+ return gravityWout;
+ }
+
+ /**
+ * \brief Sets orientation to identity and position to (0, 0, 0)
+ */
+ void setIdentity()
+ {
+ euler_.setIdentity();
+ quat_.setIdentity();
+ position().setZero();
+ }
+
+ void setRandom()
+ {
+ euler_.setRandom();
+ quat_ = euler_;
+ position().toImplementation().setRandom();
+ }
+
+ STORAGE_TYPE getStorageType() const { return storage_; }
+private:
+ bool storedAsEuler() const
+ {
+ if (storage_ == EULER)
+ return true;
+ else
+ return false;
+ }
+
+ STORAGE_TYPE storage_;
+
+ kindr::RotationQuaternion<SCALAR> quat_;
+ kindr::EulerAnglesXyz<SCALAR> euler_;
+
+ Position3Tpl position_;
+};
+
+} // namespace tpl
+
+typedef tpl::RigidBodyPose<double> RigidBodyPose;
+
+
+} // namespace rbd
+} // namespace ct
diff --git a/ct_rbd/include/ct/rbd/state/RigidBodyState.h b/ct_rbd/include/ct/rbd/state/RigidBodyState.h
new file mode 100644
index 0000000..ff8bfcb
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/state/RigidBodyState.h
@@ -0,0 +1,88 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#ifndef INCLUDE_CT_RBD_ROBOT_STATE_RIGIDBODYSTATE_H_
+#define INCLUDE_CT_RBD_ROBOT_STATE_RIGIDBODYSTATE_H_
+
+#include "RigidBodyPose.h"
+#include "RigidBodyVelocities.h"
+
+namespace ct {
+namespace rbd {
+namespace tpl {
+
+/**
+ * \brief State (pose and velocities) of a single rigid body
+ *
+ * \ingroup State
+ *
+ */
+template <typename SCALAR = double>
+class RigidBodyState
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ RigidBodyState(typename RigidBodyPose<SCALAR>::STORAGE_TYPE storage = RigidBodyPose<SCALAR>::EULER)
+ : pose_(storage), velocities_()
+ {
+ }
+
+ //! copy constructor
+ RigidBodyState(const RigidBodyState<SCALAR>& arg) : pose_(arg.pose_), velocities_(arg.velocities_) {}
+ //! destructor
+ virtual ~RigidBodyState(){};
+
+ //! return the rigid body pose
+ RigidBodyPose<SCALAR>& pose() { return pose_; }
+ //! return the rigid body pose (const)
+ const RigidBodyPose<SCALAR>& pose() const { return pose_; }
+ //! return the rigid body velocities
+ RigidBodyVelocities<SCALAR>& velocities() { return velocities_; }
+ //! return the rigid body velocities (const)
+ const RigidBodyVelocities<SCALAR>& velocities() const { return velocities_; }
+ //! numerically comparing two rigid body states
+ bool isApprox(const RigidBodyState& rhs, const double& tol = 1e-10)
+ {
+ return pose().isNear(rhs.pose(), tol) && velocities().getVector().isApprox(rhs.velocities().getVector(), tol);
+ }
+
+ /// @brief get translational velocity
+ const kindr::Velocity<SCALAR, 3> computeTranslationalVelocityW() const
+ {
+ return pose().rotateBaseToInertia(velocities().getTranslationalVelocity());
+ }
+ kindr::Velocity<SCALAR, 3> computeTranslationalVelocityW()
+ {
+ return pose().rotateBaseToInertia(velocities().getTranslationalVelocity());
+ }
+
+ void setIdentity()
+ {
+ pose().setIdentity();
+ velocities().setZero();
+ }
+
+ void setRandom()
+ {
+ pose().setRandom();
+ velocities().getTranslationalVelocity().toImplementation().setRandom();
+ velocities().getRotationalVelocity().toImplementation().setRandom();
+ }
+
+private:
+ RigidBodyPose<SCALAR> pose_;
+ RigidBodyVelocities<SCALAR> velocities_;
+};
+
+} // namespace tpl
+
+typedef tpl::RigidBodyState<double> RigidBodyState;
+
+} /* namespace rbd */
+} /* namespace ct */
+
+#endif /* INCLUDE_CT_RBD_ROBOT_STATE_RIGIDBODYSTATE_H_ */
diff --git a/ct_rbd/include/ct/rbd/state/RigidBodyVelocities.h b/ct_rbd/include/ct/rbd/state/RigidBodyVelocities.h
new file mode 100644
index 0000000..0ad8813
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/state/RigidBodyVelocities.h
@@ -0,0 +1,53 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace rbd {
+namespace tpl {
+
+/**
+ * \brief Representation of Rigid Body Velocities, currently just a typedef
+ * \ingroup State
+ */
+// unfortunately kindr stores linear velocity first and not rotational velocity.
+// so we cannot use it directly. Instead we privately inherit and make the "safe"
+// methods public and "override" the others.
+template <typename SCALAR = double>
+class RigidBodyVelocities : private kindr::TwistLinearVelocityLocalAngularVelocity<SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR, 6, 1> Vector6;
+
+ using kindr::TwistLinearVelocityLocalAngularVelocity<SCALAR>::getTranslationalVelocity;
+ using kindr::TwistLinearVelocityLocalAngularVelocity<SCALAR>::getRotationalVelocity;
+ using kindr::TwistLinearVelocityLocalAngularVelocity<SCALAR>::setZero;
+
+ Vector6 getVector() const
+ {
+ Vector6 vector6;
+ vector6 << getRotationalVelocity().toImplementation(), getTranslationalVelocity().toImplementation();
+ return vector6;
+ }
+
+ void setVector(const Vector6& vector6)
+ {
+ getRotationalVelocity().toImplementation() = vector6.template head<3>();
+ getTranslationalVelocity().toImplementation() = vector6.template tail<3>();
+ }
+
+private:
+};
+
+} // namespace tpl
+
+typedef tpl::RigidBodyVelocities<double> RigidBodyVelocities;
+
+} /* namespace rbd */
+} /* namespace ct */
diff --git a/ct_rbd/include/ct/rbd/systems/FixBaseFDSystem.h b/ct_rbd/include/ct/rbd/systems/FixBaseFDSystem.h
new file mode 100644
index 0000000..495d5ae
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/systems/FixBaseFDSystem.h
@@ -0,0 +1,243 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/rbd/state/RigidBodyPose.h>
+#include <ct/rbd/robot/actuator/ActuatorDynamics.h>
+
+#include "RBDSystem.h"
+
+
+#define ACTUATOR_DYNAMICS_ENABLED \
+ template <size_t ACT_STATE_DIMS> \
+ typename std::enable_if<(ACT_STATE_DIMS > 0), void>::type
+#define ACTUATOR_DYNAMICS_DISABLED \
+ template <size_t ACT_STATE_DIMS> \
+ typename std::enable_if<(ACT_STATE_DIMS <= 0), void>::type
+
+namespace ct {
+namespace rbd {
+
+/**
+ * \brief A fix base rigid body system that uses forward dynamics.
+ *
+ * A fix base rigid body system that uses forward dynamics. The control input vector is assumed to consist of
+ * - joint torques and
+ * - end effector forces expressed in the world frame
+ *
+ * The overall state vector is arranged in the order
+ * - joint positions
+ * - joint velocities
+ * - actuator state
+ *
+ */
+template <class RBDDynamics, size_t ACT_STATE_DIM = 0, bool EE_ARE_CONTROL_INPUTS = false>
+class FixBaseFDSystem
+ : public RBDSystem<RBDDynamics, false>,
+ public core::ControlledSystem<RBDDynamics::NSTATE + ACT_STATE_DIM, // state-dim
+ RBDDynamics::NJOINTS + EE_ARE_CONTROL_INPUTS * RBDDynamics::N_EE * 3, // input-dim of combined system
+ typename RBDDynamics::SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ static const size_t N_EE = RBDDynamics::N_EE; // number of end-effectors
+ static const size_t STATE_DIM = RBDDynamics::NSTATE + ACT_STATE_DIM; // RBD state-dim plus actuator state-dim
+ static const size_t CONTROL_DIM =
+ RBDDynamics::NJOINTS + EE_ARE_CONTROL_INPUTS * RBDDynamics::N_EE * 3; // torques plus EE-forces
+ static const size_t ACTUATOR_STATE_DIM = ACT_STATE_DIM;
+ static const size_t NJOINTS = RBDDynamics::NJOINTS;
+
+ using Dynamics = RBDDynamics;
+ using SCALAR = typename RBDDynamics::SCALAR;
+ using Base = core::ControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>;
+ using ActuatorDynamics_t = ActuatorDynamics<ACT_STATE_DIM, NJOINTS, SCALAR>;
+
+ // typedefs state and controls on all occurring dimensions
+ using state_vector_full_t = core::StateVector<STATE_DIM, SCALAR>;
+ using control_vector_full_t = core::ControlVector<CONTROL_DIM, SCALAR>;
+ using state_vector_rbd_t = core::StateVector<RBDDynamics::NSTATE, SCALAR>;
+ using state_vector_act_t = core::StateVector<ACTUATOR_STATE_DIM, SCALAR>;
+
+ //! constructor
+ /*!
+ * \warning when using actuator dynamics, the system losses its second order characteristics
+ */
+ FixBaseFDSystem() : Base(), dynamics_(RBDDynamics()), actuatorDynamics_(nullptr) { basePose_.setIdentity(); }
+ //! constructor including actuator dynamics
+ /*!
+ * \warning when using actuator dynamics, the system losses its second order characteristics
+ */
+ FixBaseFDSystem(std::shared_ptr<ActuatorDynamics_t> actuatorDynamics)
+ : Base(), dynamics_(RBDDynamics()), actuatorDynamics_(actuatorDynamics)
+ {
+ basePose_.setIdentity();
+ }
+
+ /*!
+ * @brief copy constructor
+ *
+ * @param arg instance of FixBaseFDSystem to be copied.
+ *
+ * \note takes care of explicitly cloning actuatorDynamics, if existent
+ */
+ FixBaseFDSystem(const FixBaseFDSystem& arg) : Base(arg), basePose_(arg.basePose_), dynamics_(RBDDynamics())
+ {
+ if (arg.actuatorDynamics_)
+ {
+ actuatorDynamics_ = std::shared_ptr<ActuatorDynamics_t>(arg.actuatorDynamics_->clone());
+ }
+ }
+
+ //! destructor
+ virtual ~FixBaseFDSystem() {}
+ //! get dynamics
+ virtual RBDDynamics& dynamics() override { return dynamics_; }
+ //! get dynamics (const)
+ virtual const RBDDynamics& dynamics() const override { return dynamics_; }
+ void computeControlledDynamics(const ct::core::StateVector<STATE_DIM, SCALAR>& state,
+ const SCALAR& t,
+ const ct::core::ControlVector<CONTROL_DIM, SCALAR>& controlIn,
+ ct::core::StateVector<STATE_DIM, SCALAR>& derivative)
+ {
+ // map the joint velocities
+ derivative.template topRows<NJOINTS>() = state.template segment<NJOINTS>(NJOINTS);
+
+ // temporary variable for the control (will get modified by the actuator dynamics, if applicable)
+ control_vector_full_t control = controlIn;
+
+ // extract the current RBD joint state from the state vector // todo make method to get joint state
+ typename RBDDynamics::JointState_t jState = jointStateFromVector(state);
+
+ // compute actuator dynamics and their control output
+ computeActuatorDynamics<ACT_STATE_DIM>(jState, t, state, controlIn, derivative, control);
+
+ // Cache updated rbd state
+ typename RBDDynamics::ExtLinkForces_t linkForces(Eigen::Matrix<SCALAR, 6, 1>::Zero());
+
+ // add end effector forces as control inputs (if applicable)
+ if (EE_ARE_CONTROL_INPUTS == true)
+ {
+ for (size_t i = 0; i < RBDDynamics::N_EE; i++)
+ {
+ auto endEffector = dynamics_.kinematics().getEndEffector(i);
+ size_t linkId = endEffector.getLinkId();
+ linkForces[static_cast<typename RBDDynamics::ROBCOGEN::LinkIdentifiers>(linkId)] =
+ dynamics_.kinematics().mapForceFromWorldToLink3d(
+ control.template segment<3>(RBDDynamics::NJOINTS + i * 3), basePose_, jState.getPositions(), i);
+ }
+ }
+
+ typename RBDDynamics::JointAcceleration_t jAcc;
+
+ dynamics_.FixBaseForwardDynamics(jState, control.template head<RBDDynamics::NJOINTS>(), linkForces, jAcc);
+
+ derivative.template segment<RBDDynamics::NJOINTS>(NJOINTS) = jAcc.getAcceleration();
+ }
+
+ ACTUATOR_DYNAMICS_ENABLED computeActuatorDynamics(const typename RBDDynamics::JointState_t& jState,
+ const SCALAR& t,
+ const state_vector_full_t& state,
+ const control_vector_full_t& controlIn,
+ state_vector_full_t& stateDerivative,
+ control_vector_full_t& controlOut)
+ {
+ // get references to the current actuator position, velocity and input
+ const Eigen::Ref<const typename state_vector_act_t::Base> actState = state.template tail<ACT_STATE_DIM>();
+ const Eigen::Ref<const typename control_vector_full_t::Base> actControlIn =
+ controlIn.template topRows<NJOINTS>();
+
+ state_vector_act_t actStateDerivative;
+ actuatorDynamics_->computeActuatorDynamics(jState, actState, t, actControlIn, actStateDerivative);
+
+ stateDerivative.template tail<ACTUATOR_STATE_DIM>() = actStateDerivative;
+
+ // overwrite control with actuator control output as a function of current robot and actuator states
+ controlOut = actuatorDynamics_->computeControlOutput(jState, actState);
+ }
+
+
+ // do nothing if actuators disabled
+ ACTUATOR_DYNAMICS_DISABLED computeActuatorDynamics(const typename RBDDynamics::JointState_t& jState,
+ const SCALAR& t,
+ const state_vector_full_t& state,
+ const control_vector_full_t& controlIn,
+ state_vector_full_t& stateDerivative,
+ control_vector_full_t& controlOut)
+ {
+ }
+
+ //! deep cloning
+ virtual FixBaseFDSystem<RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS>* clone() const override
+ {
+ return new FixBaseFDSystem<RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS>(*this);
+ }
+
+ //! full vector to ct JointState
+ static const typename RBDDynamics::JointState_t jointStateFromVector(const state_vector_full_t& x_full)
+ {
+ typename RBDDynamics::JointState_t jointState;
+ jointState.getPositions() = x_full.template head<RBDDynamics::NJOINTS>();
+ jointState.getVelocities() = x_full.template segment<RBDDynamics::NJOINTS>(NJOINTS);
+ return jointState;
+ }
+
+ //! transform control systems state vector to a RBDState
+ static const typename RBDDynamics::RBDState_t RBDStateFromVector(const state_vector_full_t& x_full,
+ const tpl::RigidBodyPose<SCALAR>& basePose = tpl::RigidBodyPose<SCALAR>())
+ {
+ typename RBDDynamics::RBDState_t rbdState;
+ rbdState.setZero();
+ rbdState.basePose() = basePose;
+ rbdState.joints() = jointStateFromVector(x_full);
+ return rbdState;
+ }
+
+ //! transform control systems state vector to a RBDState
+ static const state_vector_rbd_t rbdStateFromVector(const state_vector_full_t& x_full)
+ {
+ state_vector_rbd_t x_small;
+ x_small.template head<NJOINTS>() = x_full.template head<NJOINTS>();
+ x_small.template tail<NJOINTS>() = x_full.template segment<NJOINTS>(NJOINTS);
+ return x_small;
+ }
+
+ //! transform control systems state vector to the pure actuator state
+ static const state_vector_act_t actuatorStateFromVector(const state_vector_full_t& state)
+ {
+ state_vector_act_t actState = state.template tail<ACT_STATE_DIM>();
+ return actState;
+ }
+
+ //! transform from "reduced" to full coordinates including actuator dynamics
+ static const state_vector_full_t toFullState(const state_vector_rbd_t& x_rbd,
+ const state_vector_act_t& x_act = state_vector_act_t::Zero())
+ {
+ state_vector_full_t fullState;
+ fullState.template head<2 * NJOINTS>() = x_rbd.template head<2 * NJOINTS>();
+ fullState.template tail<ACTUATOR_STATE_DIM>() = x_act;
+ return fullState;
+ }
+
+ //! get pointer to actuator dynamics
+ std::shared_ptr<ActuatorDynamics_t> getActuatorDynamics() { return actuatorDynamics_; }
+private:
+ //! a "dummy" base pose which sets the robot's "fixed" position in the world
+ tpl::RigidBodyPose<SCALAR> basePose_;
+
+ //! rigid body dynamics container
+ RBDDynamics dynamics_;
+
+ //! pointer to the actuator dynamics
+ std::shared_ptr<ActuatorDynamics_t> actuatorDynamics_;
+};
+
+} // namespace rbd
+} // namespace ct
+
+#undef ACTUATOR_DYNAMICS_ENABLED
+#undef ACTUATOR_DYNAMICS_DISABLED
diff --git a/ct_rbd/include/ct/rbd/systems/FixBaseFDSystemSymplectic.h b/ct_rbd/include/ct/rbd/systems/FixBaseFDSystemSymplectic.h
new file mode 100644
index 0000000..60ef0b6
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/systems/FixBaseFDSystemSymplectic.h
@@ -0,0 +1,314 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/rbd/state/RigidBodyPose.h>
+#include <ct/rbd/robot/actuator/ActuatorDynamicsSymplectic.h>
+
+#include "RBDSystem.h"
+
+
+#define ACTUATOR_DYNAMICS_ENABLED \
+ template <size_t ACT_STATE_DIMS> \
+ typename std::enable_if<(ACT_STATE_DIMS > 0), void>::type
+#define ACTUATOR_DYNAMICS_DISABLED \
+ template <size_t ACT_STATE_DIMS> \
+ typename std::enable_if<(ACT_STATE_DIMS <= 0), void>::type
+
+namespace ct {
+namespace rbd {
+
+/**
+ * \brief A fix base rigid body system that uses forward dynamics.
+ *
+ * A fix base rigid body system that uses forward dynamics. The control input vector is assumed to consist of
+ * - joint torques and
+ * - end effector forces expressed in the world frame
+ *
+ * The state vectors of the pure RBD system and the actuator dynamics are split such that
+ * the overall system can be considered symplectic. The state vector
+ * therefore reads as
+ *
+ * \f[
+ * ( \theta_{joints}^\top p_{act}^\top \dot \theta{joints}^\top v_{act}^\top )^\top
+ * \f]
+ *
+ * \warning Make sure the actuator dynamics can be written as a symplectic system
+ * in case of using a symplectic integrator.
+ */
+template <class RBDDynamics, size_t ACT_STATE_DIM = 0, bool EE_ARE_CONTROL_INPUTS = false>
+class FixBaseFDSystemSymplectic
+ : public RBDSystem<RBDDynamics, false>,
+ public core::SymplecticSystem<RBDDynamics::NJOINTS + ACT_STATE_DIM / 2, // p-dim of combined system
+ RBDDynamics::NJOINTS + ACT_STATE_DIM / 2, // v-dim of combined system
+ RBDDynamics::NJOINTS + EE_ARE_CONTROL_INPUTS * RBDDynamics::N_EE * 3, // input-dim of combined system
+ typename RBDDynamics::SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ static const size_t N_EE = RBDDynamics::N_EE; // number of end-effectors
+ static const size_t STATE_DIM = RBDDynamics::NSTATE + ACT_STATE_DIM; // RBD state-dim plus actuator state-dim
+ static const size_t CONTROL_DIM =
+ RBDDynamics::NJOINTS + EE_ARE_CONTROL_INPUTS * RBDDynamics::N_EE * 3; // torques plus EE-forces
+ static const size_t ACTUATOR_STATE_DIM = ACT_STATE_DIM;
+ static const size_t NJOINTS = RBDDynamics::NJOINTS;
+
+ using Dynamics = RBDDynamics;
+ using SCALAR = typename RBDDynamics::SCALAR;
+ using Base = core::SymplecticSystem<STATE_DIM / 2, STATE_DIM / 2, CONTROL_DIM, SCALAR>;
+ using ActuatorDynamics_t = ActuatorDynamicsSymplectic<RBDDynamics::NJOINTS, ACT_STATE_DIM, SCALAR>;
+
+ // typedefs state and controls on all occurring dimensions
+ using state_vector_full_t = core::StateVector<STATE_DIM, SCALAR>;
+ using control_vector_full_t = core::ControlVector<CONTROL_DIM, SCALAR>;
+ using state_vector_rbd_t = core::StateVector<RBDDynamics::NSTATE, SCALAR>;
+ using state_vector_act_t = core::StateVector<ACTUATOR_STATE_DIM, SCALAR>;
+ using p_vector_full_t = core::StateVector<STATE_DIM / 2, SCALAR>;
+ using v_vector_full_t = core::StateVector<STATE_DIM / 2, SCALAR>;
+ using p_vector_rbd_t = core::StateVector<RBDDynamics::NSTATE / 2, SCALAR>;
+ using v_vector_rbd_t = core::StateVector<RBDDynamics::NSTATE / 2, SCALAR>;
+ using p_vector_act_t = core::StateVector<ACTUATOR_STATE_DIM / 2, SCALAR>;
+ using v_vector_act_t = core::StateVector<ACTUATOR_STATE_DIM / 2, SCALAR>;
+
+
+ //! constructor
+ FixBaseFDSystemSymplectic()
+ : Base(core::SYSTEM_TYPE::SECOND_ORDER), dynamics_(RBDDynamics()), actuatorDynamics_(nullptr)
+ {
+ basePose_.setIdentity();
+ }
+
+ //! constructor including actuator dynamics
+ FixBaseFDSystemSymplectic(std::shared_ptr<ActuatorDynamics_t> actuatorDynamics)
+ : Base(core::SYSTEM_TYPE::SECOND_ORDER), dynamics_(RBDDynamics()), actuatorDynamics_(actuatorDynamics)
+ {
+ basePose_.setIdentity();
+ }
+
+ /*!
+ * @brief copy constructor
+ *
+ * @param arg instance of FixBaseFDSystemSymplectic to be copied.
+ *
+ * \note takes care of explicitly cloning actuatorDynamics, if existent
+ */
+ FixBaseFDSystemSymplectic(const FixBaseFDSystemSymplectic& arg)
+ : Base(arg), basePose_(arg.basePose_), dynamics_(RBDDynamics())
+ {
+ if (arg.actuatorDynamics_)
+ {
+ actuatorDynamics_ = std::shared_ptr<ActuatorDynamics_t>(arg.actuatorDynamics_->clone());
+ }
+ }
+
+ //! destructor
+ virtual ~FixBaseFDSystemSymplectic() {}
+ //! get dynamics
+ virtual RBDDynamics& dynamics() override { return dynamics_; }
+ //! get dynamics (const)
+ virtual const RBDDynamics& dynamics() const override { return dynamics_; }
+ //! compute position derivatives, for both RBD system and actuator dynamics, if applicable
+ virtual void computePdot(const state_vector_full_t& x,
+ const v_vector_full_t& v,
+ const control_vector_full_t& controlIn,
+ p_vector_full_t& pDot) override
+ {
+ // the top rows hold the RBD velocities ...
+ pDot.template topRows<RBDDynamics::NJOINTS>() = v.template topRows<RBDDynamics::NJOINTS>();
+
+ // full joint state vector
+ typename RBDDynamics::JointState_t jState = jointStateFromVector(x);
+
+ // compute he pDot for the actuator part
+ computeActuatorPdot<ACT_STATE_DIM>(jState, x, v, controlIn, pDot);
+ }
+
+
+ ACTUATOR_DYNAMICS_ENABLED computeActuatorPdot(const typename RBDDynamics::JointState_t& jState,
+ const state_vector_full_t& x,
+ const v_vector_full_t& v,
+ const control_vector_full_t& controlIn,
+ p_vector_full_t& pDot)
+ {
+ // get const references to the current actuator position, velocity, and actuator input
+ const Eigen::Ref<const typename p_vector_act_t::Base> actPos =
+ x.template segment<ACT_STATE_DIM / 2>(RBDDynamics::NJOINTS);
+ const Eigen::Ref<const typename v_vector_act_t::Base> actVel = v.template bottomRows<ACT_STATE_DIM / 2>();
+ const Eigen::Ref<const typename control_vector_full_t::Base> actControlIn =
+ controlIn.template topRows<RBDDynamics::NJOINTS>();
+
+ // assemble temporary actuator state
+ state_vector_act_t actState;
+ actState << actPos, actVel;
+
+ p_vector_act_t actPdot;
+ actuatorDynamics_->computePdot(jState, actState, actVel, actControlIn, actPdot);
+
+ // ... the bottom rows hold the actuator dynamics
+ pDot.template bottomRows<ACT_STATE_DIM / 2>() = actPdot;
+ }
+
+
+ ACTUATOR_DYNAMICS_DISABLED computeActuatorPdot(const typename RBDDynamics::JointState_t& jState,
+ const state_vector_full_t& x,
+ const v_vector_full_t& v,
+ const control_vector_full_t& controlIn,
+ p_vector_full_t& pDot)
+ {
+ }
+
+
+ //! compute velocity derivatives, for both RBD system and actuator dynamics
+ virtual void computeVdot(const state_vector_full_t& x,
+ const p_vector_full_t& p,
+ const control_vector_full_t& controlIn,
+ v_vector_full_t& vDot) override
+ {
+ // temporary variable for the control (will get modified by the actuator dynamics, if applicable)
+ control_vector_full_t control = controlIn;
+
+ // extract the current RBD joint state from the state vector // todo make method to get joint state
+ typename RBDDynamics::JointState_t jState = jointStateFromVector(x);
+
+ computeActuatorVdot<ACT_STATE_DIM>(jState, x, p, controlIn, vDot, control);
+
+ // Cache updated rbd state
+ typename RBDDynamics::ExtLinkForces_t linkForces(Eigen::Matrix<SCALAR, 6, 1>::Zero());
+
+ // add end effector forces as control inputs (if applicable)
+ if (EE_ARE_CONTROL_INPUTS == true)
+ {
+ for (size_t i = 0; i < RBDDynamics::N_EE; i++)
+ {
+ auto endEffector = dynamics_.kinematics().getEndEffector(i);
+ size_t linkId = endEffector.getLinkId();
+ linkForces[static_cast<typename RBDDynamics::ROBCOGEN::LinkIdentifiers>(linkId)] =
+ dynamics_.kinematics().mapForceFromWorldToLink3d(
+ control.template segment<3>(RBDDynamics::NJOINTS + i * 3), basePose_, jState.getPositions(), i);
+ }
+ }
+
+ typename RBDDynamics::JointAcceleration_t jAcc;
+
+ dynamics_.FixBaseForwardDynamics(jState, control.template head<RBDDynamics::NJOINTS>(), linkForces, jAcc);
+
+ vDot.template topRows<RBDDynamics::NJOINTS>() = jAcc.getAcceleration();
+ }
+
+
+ ACTUATOR_DYNAMICS_ENABLED computeActuatorVdot(const typename RBDDynamics::JointState_t& jState,
+ const state_vector_full_t& x,
+ const p_vector_full_t& p,
+ const control_vector_full_t& controlIn,
+ v_vector_full_t& vDot,
+ control_vector_full_t& controlOut)
+ {
+ // get references to the current actuator position, velocity and input
+ const Eigen::Ref<const typename p_vector_act_t::Base> actPos =
+ p.template segment<ACT_STATE_DIM / 2>(RBDDynamics::NJOINTS);
+ const Eigen::Ref<const typename v_vector_act_t::Base> actVel = x.template bottomRows<ACT_STATE_DIM / 2>();
+ const Eigen::Ref<const typename control_vector_full_t::Base> actControlIn =
+ controlIn.template topRows<RBDDynamics::NJOINTS>();
+
+ // assemble temporary actuator state
+ state_vector_act_t actState;
+ actState << actPos, actVel;
+
+ // ... the bottom rows hold the actuator dynamics
+ v_vector_act_t actVdot;
+ actuatorDynamics_->computeVdot(jState, actState, actPos, actControlIn, actVdot);
+ vDot.template bottomRows<ACT_STATE_DIM / 2>() = actVdot;
+
+ // overwrite control with actuator control output as a function of current robot and actuator states
+ controlOut = actuatorDynamics_->computeControlOutput(jState, actState);
+ }
+
+
+ ACTUATOR_DYNAMICS_DISABLED computeActuatorVdot(const typename RBDDynamics::JointState_t& jState,
+ const state_vector_full_t& x,
+ const p_vector_full_t& p,
+ const control_vector_full_t& controlIn,
+ v_vector_full_t& vDot,
+ control_vector_full_t& controlOut)
+ {
+ }
+
+
+ //! deep cloning
+ virtual FixBaseFDSystemSymplectic<RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS>* clone() const override
+ {
+ return new FixBaseFDSystemSymplectic<RBDDynamics, ACT_STATE_DIM, EE_ARE_CONTROL_INPUTS>(*this);
+ }
+
+ //! full vector to ct JointState
+ static const typename RBDDynamics::JointState_t jointStateFromVector(const state_vector_full_t& x_full)
+ {
+ typename RBDDynamics::JointState_t jointState;
+ jointState.getPositions() = x_full.template head<RBDDynamics::NJOINTS>();
+ jointState.getVelocities() = x_full.template segment<RBDDynamics::NJOINTS>(STATE_DIM / 2);
+ return jointState;
+ }
+
+ //! transform control systems state vector to a RBDState
+ static const typename RBDDynamics::RBDState_t RBDStateFromVector(const state_vector_full_t& x_full,
+ const tpl::RigidBodyPose<SCALAR>& basePose = tpl::RigidBodyPose<SCALAR>())
+ {
+ typename RBDDynamics::RBDState_t rbdState;
+ rbdState.setZero();
+ rbdState.basePose() = basePose;
+ rbdState.joints() = jointStateFromVector(x_full);
+ return rbdState;
+ }
+
+ //! transform control systems state vector to a RBDState
+ static const state_vector_rbd_t rbdStateFromVector(const state_vector_full_t& x_full)
+ {
+ state_vector_rbd_t x_small;
+ x_small.template head<NJOINTS>() = x_full.template head<NJOINTS>();
+ x_small.template tail<NJOINTS>() = x_full.template segment<NJOINTS>(STATE_DIM / 2);
+ return x_small;
+ }
+
+ //! transform control systems state vector to the pure actuator state
+ static const state_vector_act_t actuatorStateFromVector(const state_vector_full_t& state)
+ {
+ state_vector_act_t actState;
+ actState.template topRows<ACT_STATE_DIM / 2>() =
+ state.template segment<ACT_STATE_DIM / 2>(RBDDynamics::NSTATE / 2);
+ actState.template bottomRows<ACT_STATE_DIM / 2>() = state.template bottomRows<ACT_STATE_DIM / 2>();
+ return actState;
+ }
+
+ //! transform from "reduced" to full coordinates including actuator dynamics
+ static const state_vector_full_t toFullState(const state_vector_rbd_t& x_rbd,
+ const state_vector_act_t& x_act = state_vector_act_t::Zero())
+ {
+ state_vector_full_t fullState;
+ fullState.template head<NJOINTS>() = x_rbd.template head<NJOINTS>();
+ fullState.template segment<ACTUATOR_STATE_DIM / 2>(NJOINTS) = x_act.template head<ACTUATOR_STATE_DIM / 2>();
+ fullState.template segment<NJOINTS>(STATE_DIM / 2) = x_rbd.template tail<NJOINTS>();
+ fullState.template tail<ACTUATOR_STATE_DIM / 2>() = x_act.template tail<ACTUATOR_STATE_DIM / 2>();
+ return fullState;
+ }
+
+private:
+ //! a "dummy" base pose which sets the robot's "fixed" position in the world
+ tpl::RigidBodyPose<SCALAR> basePose_;
+
+ //! rigid body dynamics container
+ RBDDynamics dynamics_;
+
+ //! pointer to the actuator dynamics
+ std::shared_ptr<ActuatorDynamics_t> actuatorDynamics_;
+};
+
+} // namespace rbd
+} // namespace ct
+
+#undef ACTUATOR_DYNAMICS_ENABLED
+#undef ACTUATOR_DYNAMICS_DISABLED
diff --git a/ct_rbd/include/ct/rbd/systems/FloatingBaseFDSystem.h b/ct_rbd/include/ct/rbd/systems/FloatingBaseFDSystem.h
new file mode 100644
index 0000000..7cb71d9
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/systems/FloatingBaseFDSystem.h
@@ -0,0 +1,191 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/rbd/state/RigidBodyPose.h>
+#include <ct/rbd/physics/EEContactModel.h>
+
+#include "RBDSystem.h"
+
+namespace ct {
+namespace rbd {
+
+/**
+ * \brief A floating base rigid body system that uses forward dynamics. The input vector
+ * is assumed to consist of joint torques and end-effector forces expressed in the world.
+ */
+template <class RBDDynamics, bool QUAT_INTEGRATION = false, bool EE_ARE_CONTROL_INPUTS = false>
+class FloatingBaseFDSystem : public RBDSystem<RBDDynamics, QUAT_INTEGRATION>,
+ public core::SymplecticSystem<RBDDynamics::NSTATE / 2 + QUAT_INTEGRATION,
+ RBDDynamics::NSTATE / 2,
+ RBDDynamics::NJOINTS + EE_ARE_CONTROL_INPUTS * RBDDynamics::N_EE * 3,
+ typename RBDDynamics::SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ using Dynamics = RBDDynamics;
+ using Kinematics = typename RBDDynamics::Kinematics_t;
+
+ typedef typename RBDDynamics::SCALAR SCALAR;
+
+ const static size_t N_EE = RBDDynamics::N_EE;
+ const static size_t STATE_DIM = RBDDynamics::NSTATE + QUAT_INTEGRATION;
+ const static size_t CONTROL_DIM = RBDDynamics::NJOINTS + EE_ARE_CONTROL_INPUTS * N_EE * 3;
+
+ typedef core::StateVector<STATE_DIM, SCALAR> StateVector;
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> ControlVector;
+
+ typedef core::
+ SymplecticSystem<RBDDynamics::NSTATE / 2 + QUAT_INTEGRATION, RBDDynamics::NSTATE / 2, CONTROL_DIM, SCALAR>
+ Base;
+
+ using ContactModel = ct::rbd::EEContactModel<Kinematics>;
+
+ FloatingBaseFDSystem() : Base(), dynamics_(), eeContactModel_(nullptr) {}
+ FloatingBaseFDSystem(const FloatingBaseFDSystem<RBDDynamics, QUAT_INTEGRATION, EE_ARE_CONTROL_INPUTS>& other)
+ : Base(other), dynamics_(other.dynamics_), eeContactModel_(other.eeContactModel_->clone())
+ {
+ }
+
+ virtual ~FloatingBaseFDSystem() {}
+ virtual FloatingBaseFDSystem<RBDDynamics, QUAT_INTEGRATION, EE_ARE_CONTROL_INPUTS>* clone() const override
+ {
+ return new FloatingBaseFDSystem<RBDDynamics, QUAT_INTEGRATION, EE_ARE_CONTROL_INPUTS>(*this);
+ }
+
+ virtual RBDDynamics& dynamics() override { return dynamics_; }
+ virtual const RBDDynamics& dynamics() const override { return dynamics_; }
+ void setContactModel(const std::shared_ptr<ContactModel>& contactModel) { eeContactModel_ = contactModel; }
+ virtual void computePdot(const StateVector& x,
+ const core::StateVector<RBDDynamics::NSTATE / 2, SCALAR>& v,
+ const ControlVector& control,
+ core::StateVector<RBDDynamics::NSTATE / 2 + QUAT_INTEGRATION, SCALAR>& pDot) override
+ {
+ StateVector xLocal = x;
+
+ xLocal.tail(RBDDynamics::NSTATE / 2) = v;
+
+ typename RBDDynamics::RBDState_t rbdCached = RBDStateFromVector(xLocal);
+
+ typename RBDDynamics::RBDAcceleration_t xd;
+
+ pDot = toStateDerivative<QUAT_INTEGRATION>(xd, rbdCached).head(RBDDynamics::NSTATE / 2 + QUAT_INTEGRATION);
+ }
+
+ virtual void computeVdot(const StateVector& x,
+ const core::StateVector<RBDDynamics::NSTATE / 2 + QUAT_INTEGRATION, SCALAR>& p,
+ const ControlVector& control,
+ core::StateVector<RBDDynamics::NSTATE / 2, SCALAR>& vDot) override
+ {
+ StateVector xLocal = x;
+ xLocal.head(RBDDynamics::NSTATE / 2 + QUAT_INTEGRATION) = p;
+
+ // Cache updated rbd state
+ typename RBDDynamics::RBDState_t rbdCached = RBDStateFromVector(xLocal);
+ typename RBDDynamics::ExtLinkForces_t linkForces(Eigen::Matrix<SCALAR, 6, 1>::Zero());
+
+ std::array<typename Kinematics::EEForceLinear, N_EE> eeForcesW;
+ eeForcesW.fill(Kinematics::EEForceLinear::Zero());
+
+ if (eeContactModel_)
+ eeForcesW = eeContactModel_->computeContactForces(rbdCached);
+
+ if (EE_ARE_CONTROL_INPUTS)
+ for (size_t i = 0; i < N_EE; i++)
+ eeForcesW[i] += control.template segment<3>(RBDDynamics::NJOINTS + i * 3);
+
+ mapEndeffectorForcesToLinkForces(rbdCached, eeForcesW, linkForces);
+
+ typename RBDDynamics::RBDAcceleration_t xd;
+
+ dynamics_.FloatingBaseForwardDynamics(rbdCached, control.template head<RBDDynamics::NJOINTS>(), linkForces, xd);
+
+ vDot = toStateDerivative<QUAT_INTEGRATION>(xd, rbdCached).tail(RBDDynamics::NSTATE / 2);
+ }
+
+ /**
+ * Maps the end-effector forces expressed in the world to the link frame as required by robcogen.
+ * The link forces are transformed from world frame to the link frame. The according momentum is added.
+ * @param state robot state
+ * @param control end-effector forces expressed in the world
+ * @param linkForces forces acting on the link expressed in the link frame
+ */
+ void mapEndeffectorForcesToLinkForces(const typename RBDDynamics::RBDState_t& state,
+ const std::array<typename Kinematics::EEForceLinear, N_EE>& eeForcesW,
+ typename RBDDynamics::ExtLinkForces_t& linkForces)
+ {
+ for (size_t i = 0; i < N_EE; i++)
+ {
+ auto endEffector = dynamics_.kinematics().getEndEffector(i);
+ size_t linkId = endEffector.getLinkId();
+ linkForces[static_cast<typename RBDDynamics::ROBCOGEN::LinkIdentifiers>(linkId)] =
+ dynamics_.kinematics().mapForceFromWorldToLink3d(
+ eeForcesW[i], state.basePose(), state.jointPositions(), i);
+ }
+ }
+
+ typename RBDDynamics::RBDState_t RBDStateFromVector(const core::StateVector<STATE_DIM, SCALAR>& state)
+ {
+ return RBDStateFromVectorImpl<QUAT_INTEGRATION>(state);
+ }
+
+ template <bool T>
+ typename RBDDynamics::RBDState_t RBDStateFromVectorImpl(const core::StateVector<STATE_DIM, SCALAR>& state,
+ typename std::enable_if<T, bool>::type = true)
+ {
+ typename RBDDynamics::RBDState_t x(tpl::RigidBodyPose<SCALAR>::QUAT);
+ x.fromStateVectorQuaternion(state);
+ return x;
+ }
+
+ template <bool T>
+ typename RBDDynamics::RBDState_t RBDStateFromVectorImpl(const core::StateVector<STATE_DIM, SCALAR>& state,
+ typename std::enable_if<!T, bool>::type = true)
+ {
+ typename RBDDynamics::RBDState_t x(tpl::RigidBodyPose<SCALAR>::EULER);
+ x.fromStateVectorEulerXyz(state);
+ return x;
+ }
+
+ template <bool T>
+ core::StateVector<STATE_DIM, SCALAR> toStateDerivative(const typename RBDDynamics::RBDAcceleration_t& acceleration,
+ const typename RBDDynamics::RBDState_t& state,
+ typename std::enable_if<T, bool>::type = true)
+ {
+ return acceleration.toStateUpdateVectorQuaternion(state);
+ }
+
+ template <bool T>
+ core::StateVector<STATE_DIM, SCALAR> toStateDerivative(const typename RBDDynamics::RBDAcceleration_t& acceleration,
+ const typename RBDDynamics::RBDState_t& state,
+ typename std::enable_if<!T, bool>::type = true)
+ {
+ return acceleration.toStateUpdateVectorEulerXyz(state);
+ }
+
+ template <bool T>
+ core::StateVector<STATE_DIM, SCALAR> toStateVector(const typename RBDDynamics::RBDState_t& state,
+ typename std::enable_if<T, bool>::type = true)
+ {
+ return state.toStateVectorQuaternion();
+ }
+
+ template <bool T>
+ core::StateVector<STATE_DIM, SCALAR> toStateVector(const typename RBDDynamics::RBDState_t& state,
+ typename std::enable_if<!T, bool>::type = true)
+ {
+ return state.toStateVectorEulerXyz();
+ }
+
+private:
+ RBDDynamics dynamics_;
+ std::shared_ptr<ContactModel> eeContactModel_;
+};
+
+} // namespace rbd
+} // namespace ct
diff --git a/ct_rbd/include/ct/rbd/systems/ProjectedFDSystem.h b/ct_rbd/include/ct/rbd/systems/ProjectedFDSystem.h
new file mode 100644
index 0000000..a091e48
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/systems/ProjectedFDSystem.h
@@ -0,0 +1,144 @@
+
+#pragma once
+
+#include <ct/rbd/state/RigidBodyPose.h>
+#include <ct/rbd/physics/EEContactModel.h>
+
+#include "RBDSystem.h"
+
+namespace ct {
+namespace rbd {
+
+/*!
+ * \brief A floating base rigid body system that uses forward dynamics. The input vector
+ * is assumed to consist of joint torques and end-effector forces expressed in the world.
+ *
+ * This class creates a ct::core::ControlledSystem based on ProjectedDynamics to make it
+ * compatible with ct::core functionality such as a ct::core::Integrator. Please note that
+ * there are multiple choices of transforming a Rigid Body Dynamics system into an ODE used
+ * for integration and (optimal) control. This particular modelling is explained below. For
+ * other choices see FloatingBaseFDSystem and FixBaseFDSystem.
+ *
+ * The goal of this class is to transform a Rigid Body Dynamics system into an ODE of the form
+ * \f$ \dot{x} = f(x,u) \f$ using projected forward dynamics as implemented in ProjectedDynamics.
+ * To do so, we define the state as
+ * \f[
+ * x = [ {}_W q_B ~ {}_W p_B ~ \theta_J ~ {}_B \omega_B ~ {}_B v_B ~ \dot{\theta}_J ]^T
+ * \f]
+ *
+ * where \f$ {}_W q_B \f$ is the base orientation and \f$ {}_W p_B \f$ is the base position,
+ * both expressed the world frame. \f$ {}_B \omega_B \f$ is the local angular velocity of the
+ * base and \f$ {}_B v_B \f$ is the linear velocity of the base, both are expressed in the local
+ * base coordinate frame. Hence, the velocities are \b NOT direct derivatives of the position/orientation.
+ * \f$ \theta_J \f$ and \f$ \dot{theta}_J \f$ are the joint angles
+ * and joint velocities, respectively.
+ *
+ * The input vector \f$ u = \tau \f$ contains the joint torques. We further assume the base is unactuated.
+ *
+ * Given the Projected Rigid Body Dynamics
+ * \f[
+ * {}_B \ddot{q}_c = P M^{-1} (J^T_c \lambda + S^T \tau - G - C)
+ * \f]
+ *
+ * where \f$ {}_B \ddot{q}_c = [ {}_B \dot{\omega}_B ~ {}_B \dot{v}_B ~ \ddot{\theta}_J ]^T \f$
+ * are the generalized coordinates expressed in the base frame.
+ *
+ * The system dynamics then become:
+ *
+ * \f[
+ * \begin{aligned}
+ * \dot{x} &= [ {}_W \dot{q}_B ~ {}_W \dot{p}_B ~ \dot{\theta}_J ~ {}_B \dot{\omega}_B ~ {}_B \dot{v}_B ~ \ddot{\theta}_J ]^T \\
+ * &= [ H_{WB} {}_B \omega_B ~ R_{WB} {}_B v_B ~ \dot{\theta}_J ~ P M^{-1} (J^T_c \lambda + S^T \tau - G - C)]^T
+ * \end{aligned}
+ * \f]
+ */
+template <class RBDDynamics, bool QUAT_INTEGRATION = false>
+class ProjectedFDSystem : public RBDSystem<RBDDynamics, QUAT_INTEGRATION>,
+ public core::ControlledSystem<RBDDynamics::NSTATE + QUAT_INTEGRATION, RBDDynamics::NJOINTS>
+{
+public:
+ using Dynamics = RBDDynamics;
+ using Kinematics = typename RBDDynamics::Kinematics_t;
+
+ const static size_t N_EE = RBDDynamics::N_EE;
+ const static size_t STATE_DIM = RBDDynamics::NSTATE + QUAT_INTEGRATION;
+ const static size_t CONTROL_DIM = RBDDynamics::NJOINTS;
+
+ typedef core::ControlledSystem<RBDDynamics::RBDState_t::NSTATE + QUAT_INTEGRATION, RBDDynamics::NJOINTS> Base;
+
+ ProjectedFDSystem(typename RBDDynamics::EE_in_contact_t eeInContact = true) : eeInContact_(eeInContact){};
+
+ virtual ~ProjectedFDSystem(){};
+
+ virtual RBDDynamics& dynamics() override { return dynamics_; }
+ virtual const RBDDynamics& dynamics() const override { return dynamics_; }
+ virtual void computeControlledDynamics(const core::StateVector<STATE_DIM>& state,
+ const core::Time& t,
+ const core::ControlVector<CONTROL_DIM>& control,
+ core::StateVector<STATE_DIM>& derivative
+
+ ) override
+ {
+ typename RBDDynamics::RBDState_t x = RBDStateFromVector(state);
+
+ typename RBDDynamics::RBDAcceleration_t xd;
+
+ dynamics_.ProjectedForwardDynamics(eeInContact_, x, control, xd);
+
+ derivative = toStateDerivative<QUAT_INTEGRATION>(xd, x);
+ }
+
+ void setEEInContact(typename RBDDynamics::EE_in_contact_t& eeInContact) { eeInContact_ = eeInContact; }
+ typename RBDDynamics::RBDState_t RBDStateFromVector(const core::StateVector<STATE_DIM>& state)
+ {
+ return RBDStateFromVectorImpl<QUAT_INTEGRATION>(state);
+ }
+
+ template <bool T>
+ typename RBDDynamics::RBDState_t RBDStateFromVectorImpl(const core::StateVector<STATE_DIM>& state,
+ typename std::enable_if<T, bool>::type = true)
+ {
+ typename RBDDynamics::RBDState_t x(RigidBodyPose::QUAT);
+ x.fromStateVectorQuaternion(state);
+ return x;
+ }
+
+ template <bool T>
+ typename RBDDynamics::RBDState_t RBDStateFromVectorImpl(const core::StateVector<STATE_DIM>& state,
+ typename std::enable_if<!T, bool>::type = true)
+ {
+ typename RBDDynamics::RBDState_t x(RigidBodyPose::EULER);
+ x.fromStateVectorEulerXyz(state);
+ return x;
+ }
+
+ template <bool T>
+ core::StateVector<STATE_DIM> toStateDerivative(const typename RBDDynamics::RBDAcceleration_t& acceleration,
+ const typename RBDDynamics::RBDState_t& state,
+ typename std::enable_if<T, bool>::type = true)
+ {
+ return acceleration.toStateUpdateVectorQuaternion(state);
+ }
+
+ template <bool T>
+ core::StateVector<STATE_DIM> toStateDerivative(const typename RBDDynamics::RBDAcceleration_t& acceleration,
+ const typename RBDDynamics::RBDState_t& state,
+ typename std::enable_if<!T, bool>::type = true)
+ {
+ return acceleration.toStateUpdateVectorEulerXyz(state);
+ }
+
+
+ virtual ProjectedFDSystem<RBDDynamics, QUAT_INTEGRATION>* clone() const override
+ {
+ throw std::runtime_error("clone not implemented");
+ }
+
+private:
+ typename RBDDynamics::EE_in_contact_t eeInContact_;
+
+ RBDDynamics dynamics_;
+};
+
+} // namespace rbd
+} // namespace ct
diff --git a/ct_rbd/include/ct/rbd/systems/RBDSystem.h b/ct_rbd/include/ct/rbd/systems/RBDSystem.h
new file mode 100644
index 0000000..a3dfc42
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/systems/RBDSystem.h
@@ -0,0 +1,31 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#ifndef INCLUDE_CT_RBD_SYSTEMS_RBDSYSTEM_H_
+#define INCLUDE_CT_RBD_SYSTEMS_RBDSYSTEM_H_
+
+namespace ct {
+namespace rbd {
+
+/**
+ * \brief This is a common interface class for an RBDSystem
+ */
+template <class RBDDynamics, bool QUAT_INTEGRATION = false>
+class RBDSystem
+{
+public:
+ RBDSystem() {}
+ virtual ~RBDSystem() {}
+ virtual RBDDynamics& dynamics() = 0;
+ virtual const RBDDynamics& dynamics() const = 0;
+
+private:
+};
+}
+}
+
+
+#endif /* INCLUDE_CT_RBD_SYSTEMS_RBDSYSTEM_H_ */
diff --git a/ct_rbd/include/ct/rbd/systems/linear/RbdLinearizer.h b/ct_rbd/include/ct/rbd/systems/linear/RbdLinearizer.h
new file mode 100644
index 0000000..6162340
--- /dev/null
+++ b/ct_rbd/include/ct/rbd/systems/linear/RbdLinearizer.h
@@ -0,0 +1,285 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+/*!
+ * \brief System Linearizer dedicated to Articulated Rigid Body Model.
+ *
+ * \warning this Linearizer is different to the standard system linearizer
+ * in ct_core. It takes care of additional terms arising from floating-base
+ * systems. If the system is fixed-base, the standard ct_core linearizer
+ * is called.
+ *
+ */
+
+#pragma once
+
+#include <memory>
+
+#include <kindr/Core>
+
+template <int N>
+struct print_size_as_warning
+{
+ char operator()() { return N + 256; } //deliberately causing overflow
+};
+
+#define DEBUG
+
+namespace ct {
+namespace rbd {
+
+template <class SYSTEM>
+class RbdLinearizer : public ct::core::SystemLinearizer<SYSTEM::STATE_DIM, SYSTEM::CONTROL_DIM, typename SYSTEM::SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ static const bool FLOATING_BASE = SYSTEM::Dynamics::FB;
+
+ typedef std::shared_ptr<RbdLinearizer<SYSTEM>> Ptr;
+ typedef typename SYSTEM::SCALAR SCALAR;
+
+ static const size_t STATE_DIM = SYSTEM::STATE_DIM;
+ static const size_t CONTROL_DIM = SYSTEM::CONTROL_DIM;
+ static const size_t NJOINTS = SYSTEM::Dynamics::NJOINTS;
+
+ static_assert(SYSTEM::STATE_DIM == 2 * (SYSTEM::Dynamics::NJOINTS + FLOATING_BASE * 6),
+ "STATE DIMENSION MISMATCH. RBD LINEARIZER ONLY WORKS FOR PURE RIGID BODY DYNAMICS.");
+ static_assert(SYSTEM::CONTROL_DIM == SYSTEM::Dynamics::NJOINTS,
+ "CONTROL DIMENSION MISMATCH. RBD LINEARIZER ONLY WORKS FOR FULL JOINT CONTROLLED SYSTEMS.");
+
+
+ typedef ct::core::SystemLinearizer<STATE_DIM, CONTROL_DIM, SCALAR> Base;
+
+ typedef ct::core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef ct::core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
+
+ typedef ct::core::StateMatrix<STATE_DIM, SCALAR> state_matrix_t;
+ typedef ct::core::StateControlMatrix<STATE_DIM, CONTROL_DIM, SCALAR> state_control_matrix_t;
+
+
+ RbdLinearizer(std::shared_ptr<SYSTEM> RBDSystem, bool doubleSidedDerivative = false)
+ : Base(RBDSystem, doubleSidedDerivative), RBDSystem_(RBDSystem)
+ {
+ // check if a non-floating base system is a second order system
+ if (!FLOATING_BASE && (this->getType() != ct::core::SYSTEM_TYPE::SECOND_ORDER))
+ {
+ std::cout
+ << "RbdLinearizer.h: Warning, fixed base system not declared as second order system. "
+ << "Normally, fixed base systems are second order systems and RbdLinearizer will treat them like this. "
+ << "To declare system as second order, see ct::core::System" << std::endl;
+ }
+
+ // fill default
+ this->dFdx_.template topLeftCorner<STATE_DIM / 2, STATE_DIM / 2>().setZero();
+ this->dFdx_.template topRightCorner<STATE_DIM / 2, STATE_DIM / 2>().setIdentity();
+
+ // fill default
+ this->dFdu_.template topRows<STATE_DIM / 2>().setZero();
+ }
+
+ RbdLinearizer(const RbdLinearizer& arg) : Base(arg), RBDSystem_(std::shared_ptr<SYSTEM>(arg.RBDSystem_->clone())) {}
+ virtual ~RbdLinearizer() override {}
+ RbdLinearizer<SYSTEM>* clone() const override { return new RbdLinearizer<SYSTEM>(*this); }
+ const state_matrix_t& getDerivativeState(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t = 0.0) override
+ {
+ if (!FLOATING_BASE)
+ {
+ // call standard ct_core linearizer
+ return Base::getDerivativeState(x, u, t);
+ }
+ else
+ {
+ Base::getDerivativeState(x, u, t);
+
+ // since we express base pose in world but base twist in body coordinates, we have to modify the top part
+ kindr::EulerAnglesXyz<SCALAR> eulerXyz(x.template topRows<3>());
+ kindr::RotationMatrix<SCALAR> R_WB_kindr(eulerXyz);
+
+ Eigen::Matrix<SCALAR, 3, 6> jacAngVel = jacobianOfAngularVelocityMapping(x.template topRows<3>(),
+ x.template segment<3>(STATE_DIM / 2)).transpose();
+
+ //this->dFdx_.template block<3,3>(0,0) = -R_WB_kindr.toImplementation() * JacobianOfRotationMultiplyVector( x.template topRows<3>(), R_WB_kindr.toImplementation()*(x.template segment<3>(STATE_DIM/2) ));
+ this->dFdx_.template block<3, 3>(0, 0) = jacAngVel.template block<3, 3>(0, 0);
+
+ this->dFdx_.template block<3, 3>(3, 0) =
+ -R_WB_kindr.toImplementation() *
+ JacobianOfRotationMultiplyVector(x.template topRows<3>(),
+ R_WB_kindr.toImplementation() * (x.template segment<3>(STATE_DIM / 2 + 3)));
+
+
+ // Derivative Top Row
+ // This is the derivative of the orientation with respect to local angular velocity. This is NOT the rotation matrix
+ this->dFdx_.template block<3, 3>(0, STATE_DIM / 2) = jacAngVel.template block<3, 3>(0, 3);
+ // we prefer to use a combined calculation. The following call would be equivalent but recomputes sines/cosines
+ //this->dFdx_.template block<3, 3>(0, STATE_DIM/2) = eulerXyz.getMappingFromLocalAngularVelocityToDiff();
+
+ // This is the derivative of the position with respect to linear velocity. This is simply the rotation matrix
+ this->dFdx_.template block<3, 3>(3, STATE_DIM / 2 + 3) = R_WB_kindr.toImplementation();
+
+ return this->dFdx_;
+ }
+ }
+
+ const state_control_matrix_t& getDerivativeControl(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t = 0.0) override
+ {
+ const jsim_t& M = RBDSystem_->dynamics().kinematics().robcogen().jSim().update(
+ x.template segment<NJOINTS>(FLOATING_BASE * 6));
+
+ llt_.compute(M);
+
+ const typename jsim_t::MatrixType& M_inv = llt_.solve(jsim_t::Identity());
+
+#ifdef DEBUG
+ typename jsim_t::MatrixType Meigen = M;
+ if (!Meigen.inverse().isApprox(M_inv))
+ {
+ Eigen::SelfAdjointEigenSolver<typename jsim_t::MatrixType> eigensolver(M);
+ std::cout << "The eigenvalues of M are:\n" << eigensolver.eigenvalues().transpose() << std::endl;
+ std::cout << "M inverse incorrect" << std::endl;
+ std::cout << "M.inverse: " << std::endl << Meigen.inverse() << std::endl;
+ std::cout << "M_inv: " << std::endl << M_inv << std::endl;
+
+ // Marco's LLT version
+ jsim_t M_temp = M;
+ M_temp.computeL();
+ M_temp.computeInverse();
+ auto M_inv_robcogen = M_temp.getInverse();
+ std::cout << "M_inv_robcogen: " << std::endl << M_inv_robcogen << std::endl;
+ }
+#endif //DEBUG
+
+ auto& S = RBDSystem_->dynamics().S();
+
+ this->dFdu_.template block<STATE_DIM / 2, CONTROL_DIM>(STATE_DIM / 2, 0) = M_inv * S.transpose();
+
+ return this->dFdu_;
+ }
+
+
+protected:
+ typedef typename SYSTEM::Dynamics::ROBCOGEN::JSIM jsim_t;
+
+ std::shared_ptr<SYSTEM> RBDSystem_;
+
+ Eigen::LLT<typename jsim_t::MatrixType> llt_;
+
+private:
+ // auto generated code
+ Eigen::Matrix<SCALAR, 3, 3> JacobianOfRotationMultiplyVector(const Eigen::Matrix<SCALAR, 3, 1>& theta,
+ const Eigen::Matrix<SCALAR, 3, 1>& vector)
+ {
+ const SCALAR& theta_1 = theta(0);
+ const SCALAR& theta_2 = theta(1);
+ const SCALAR& theta_3 = theta(2);
+
+ const SCALAR& vector_1 = vector(0);
+ const SCALAR& vector_2 = vector(1);
+ const SCALAR& vector_3 = vector(2);
+
+
+ Eigen::Matrix<SCALAR, 3, 3> A0;
+ A0.setZero();
+
+
+ SCALAR t2 = cos(theta_1);
+ SCALAR t3 = sin(theta_3);
+ SCALAR t4 = cos(theta_3);
+ SCALAR t5 = sin(theta_1);
+ SCALAR t6 = sin(theta_2);
+ SCALAR t7 = cos(theta_2);
+ SCALAR t8 = t4 * t5;
+ SCALAR t9 = t2 * t3 * t6;
+ SCALAR t10 = t8 + t9;
+ SCALAR t11 = t2 * t4;
+ SCALAR t12 = t11 - t3 * t5 * t6;
+ SCALAR t13 = t6 * vector_1;
+ SCALAR t14 = t2 * t7 * vector_3;
+ SCALAR t15 = t13 + t14 - t5 * t7 * vector_2;
+ SCALAR t16 = t2 * t3;
+ SCALAR t17 = t4 * t5 * t6;
+ SCALAR t18 = t16 + t17;
+ SCALAR t19 = t3 * t5;
+ SCALAR t20 = t19 - t2 * t4 * t6;
+ A0(0, 0) = t18 * vector_3 - t20 * vector_2;
+ A0(0, 1) = -t4 * t15;
+ A0(0, 2) = t10 * vector_3 + t12 * vector_2 - t3 * t7 * vector_1;
+ A0(1, 0) = -t10 * vector_2 + t12 * vector_3;
+ A0(1, 1) = t3 * t15;
+ A0(1, 2) = -t18 * vector_2 - t20 * vector_3 - t4 * t7 * vector_1;
+ A0(2, 0) = -t7 * (t2 * vector_2 + t5 * vector_3);
+ A0(2, 1) = t7 * vector_1 - t2 * t6 * vector_3 + t5 * t6 * vector_2;
+
+ return A0;
+ }
+
+ /*!
+ * to map local angular velocity \f$ \omega_W \f$ expressed in body coordinates, to changes in Euler Angles expressed in an inertial frame \f$ q_I \f$
+ * we have to map them via \f$ \dot{q}_I = H \omega_W \f$, where \f$ H \f$ is the matrix defined in kindr getMappingFromLocalAngularVelocityToDiff().
+ * You can see the kindr cheat sheet to figure out how to build this matrix. The following code computes the Jacobian of \f$ \dot{q}_I \f$
+ * with respect to \f$ q_I \f$ and \f$ \omega_W \f$ . Thus the lower part of the Jacobian is H and the upper part is \f$ dH/dq_I \omega_W \f$ . We include
+ * both parts for more efficient computation. The following code is computed using auto-diff.
+ *
+ * @param eulerAnglesXyz
+ * @param angularVelocity
+ * @return
+ */
+ Eigen::Matrix<SCALAR, 6, 3> jacobianOfAngularVelocityMapping(const Eigen::Matrix<SCALAR, 3, 1>& eulerAnglesXyz,
+ const Eigen::Matrix<SCALAR, 3, 1>& angularVelocity)
+ {
+ using namespace std;
+
+ Eigen::Matrix<SCALAR, 6, 1> xAD;
+ xAD << eulerAnglesXyz, angularVelocity;
+ const SCALAR* x = xAD.data();
+
+ std::array<SCALAR, 10> v;
+
+ Eigen::Matrix<SCALAR, 6, 3> jac;
+ SCALAR* y = jac.data();
+
+ y[9] = sin(x[2]);
+ y[10] = cos(x[2]);
+ v[0] = cos(x[1]);
+ v[1] = 1 / v[0];
+ y[3] = v[1] * y[10];
+ v[2] = sin(x[1]);
+ y[1] = 0 - (0 - (0 - x[4] * y[9] + x[3] * y[10]) * 1 / v[0] * v[1]) * v[2];
+ v[3] = sin(x[2]);
+ v[4] = 0 - v[1];
+ y[4] = v[4] * y[9];
+ v[5] = y[10];
+ y[2] = 0 - x[3] * v[1] * v[3] + x[4] * v[4] * v[5];
+ y[8] = 0 - x[4] * v[3] + x[3] * v[5];
+ v[6] = v[1] * y[9];
+ v[7] = v[4] * y[10];
+ v[8] = v[2];
+ y[15] = v[7] * v[8];
+ y[16] = v[6] * v[8];
+ v[9] = x[4] * v[8];
+ v[8] = x[3] * v[8];
+ y[13] = (x[4] * v[6] + x[3] * v[7]) * v[0] - (0 - (v[9] * y[9] - v[8] * y[10]) * 1 / v[0] * v[1]) * v[2];
+ y[14] = 0 - v[8] * v[4] * v[3] + v[9] * v[1] * v[5];
+ // dependent variables without operations
+ y[0] = 0;
+ y[5] = 0;
+ y[6] = 0;
+ y[7] = 0;
+ y[11] = 0;
+ y[12] = 0;
+ y[17] = 1;
+
+
+ return jac;
+ }
+};
+
+} // rbd
+} // ct
diff --git a/ct_rbd/include/iit/rbd/InertiaMatrix.h b/ct_rbd/include/iit/rbd/InertiaMatrix.h
new file mode 100644
index 0000000..a6251c7
--- /dev/null
+++ b/ct_rbd/include/iit/rbd/InertiaMatrix.h
@@ -0,0 +1,322 @@
+/* CPYHDR { */
+/*
+ * This file is part of the 'iit-rbd' library.
+ * Copyright © 2015 2016, Marco Frigerio (marco.frigerio@iit.it)
+ *
+ * See the LICENSE file for more information.
+ */
+/* } CPYHDR */
+
+#ifndef _IIT_RBD_INERTIAMATRIX_H_
+#define _IIT_RBD_INERTIAMATRIX_H_
+
+#include "rbd.h"
+
+
+namespace iit {
+namespace rbd {
+
+namespace tpl {
+/**
+ * Dense 6x6 matrix that represents the 6D spatial inertia tensor.
+ * See chapther 2 of Featherstone's "Rigid body dynamics algorithms".
+ */
+template<typename SCALAR>
+class InertiaMatrixDense : public Core<SCALAR>::Matrix66
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef SCALAR Scalar;
+private:
+ typedef Core<Scalar> Cores;
+ typedef typename Cores::Matrix66 Base;
+ typedef typename Cores::Matrix33 Mat33;
+ typedef typename Cores::Vector3 Vec3;
+ typedef MatrixBlock<const Base,3,3> Block33_const;
+ typedef MatrixBlock<Base,3,3> Block33_t;
+
+public:
+ template<typename OtherDerived>
+ InertiaMatrixDense<SCALAR>& operator= (const MatrixBase<OtherDerived>& other);
+
+ template<typename OtherDerived>
+ InertiaMatrixDense<SCALAR>& operator+= (const MatrixBase<OtherDerived>& other);
+
+ InertiaMatrixDense();
+ /**
+ * See fill()
+ */
+ InertiaMatrixDense(Scalar m, const Vec3& com, const Mat33& I);
+
+public:
+ /**
+ * Sets this 6x6 inertia tensor according to the given inertia properties.
+ * All the values (ie the COM and the 3x3 tensor) must be expressed in the
+ * same reference frame.
+ *
+ * No consistency checks are performed (Note: possibly changing in future).
+ *
+ * \param mass the total mass of the body
+ * \param com the 3D vector with the position of the center of mass
+ * \param tensor the classical 3x3 inertia tensor; this parameter should be
+ * expressed in the same coordinate frame as the center-of-mass vector.
+ * In other words, it is NOT treated as the inertia tensor with respect
+ * to a frame with origin in the center-of-mass, and the parallel axis
+ * theorem is NOT applied. The given tensor is copied as it is, in the
+ * appropriate 3x3 sub-block of this spatial tensor.
+ */
+ void fill(Scalar m, const Vec3& com, const Mat33& tensor);
+
+ /** \name Components getters **/
+ ///@{
+ /**
+ * @return the current value of the mass of the rigid body modeled by this
+ * tensor
+ */
+ Scalar getMass() const;
+ /**
+ * @return the position of the center-of-mass of the rigid body modeled by
+ * this spatial tensor
+ */
+ Vec3 getCOM() const;
+ /**
+ * @return the 3x3 block of this spatial tensor which corresponds to the
+ * sole rotational inertia, that is, the classical inertia tensor
+ */
+ const Block33_const get3x3Tensor() const;
+ ///@}
+
+ /** \name Modifiers **/
+ ///@{
+ /**
+ * Scales the whole tensor according to the new value of the mass.
+ *
+ * The changes guarantee that the matrix remains positive definite (assuming
+ * it was so before).
+ *
+ * This method does NOT change the center-of-mass property, while it does
+ * change the moments of inertia. Intuitively, calling this method corresponds
+ * to changing the mass-density of the body leaving its size and geometry
+ * untouched.
+ *
+ * @param newMass the new value of the mass (always expressed in Kilograms);
+ * it MUST be positive, no checks are performed
+ */
+ void changeMass(Scalar m);
+ /**
+ * Changes the position of the Center-Of-Mass of the rigid body modeled by
+ * this tensor.
+ *
+ * In addition to the two off-diagonal 3x3 blocks, this method also modifies
+ * the 3x3 block that corresponds to the classical inertia tensor, to keep
+ * it consistent with the position of the center of mass. It does not change
+ * the mass property. Cfr. chapter 2 of Featherstone's book on rigid body
+ * dynamics algorithms.
+ * TODO show some equations
+ *
+ * @param newcom a 3D vector specifying the position of the center of mass,
+ * expressed in meters
+ */
+ void changeCOM(const Vec3& newcom);
+ /**
+ * Simply sets the 3x3 block that corresponds to the classical rotational
+ * inertia
+ * @param tensor the new 3x3 rotational inertia tensor
+ */
+ void changeRotationalInertia(const Mat33& tensor);
+ ///@}
+protected:
+ void setTheFixedZeros();
+
+ template<typename Vector>
+ void setSkewSymmetricBlock(
+ const MatrixBase<Vector>& v,
+ Block33_t block);
+private:
+ void set(Scalar m, const Vec3& com, const Mat33& I);
+
+};
+
+template<typename SCALAR>
+class InertiaMatrixSparse : public SparseMatrix<SCALAR>
+{
+ typedef SparseMatrix<SCALAR> Base;
+};
+
+
+#define block33 this->template block<3,3>
+#define data (this->operator())
+#define TPL template<typename S>
+#define CLASS InertiaMatrixDense<S>
+
+
+/* I do not care here about the creation of temporaries, thus I use const
+ * refs to actual Eigen matrices, rather than the common base of the
+ * matrix expression. If you haven't read Eigen docs, this comment is
+ * completely obscure!
+ */
+TPL
+inline CLASS::InertiaMatrixDense() : Base() {
+ setTheFixedZeros();
+}
+
+/**
+ * Initializes this 6x6 tensor according to the given inertia parameters.
+ * \see fill()
+ */
+TPL
+inline CLASS::InertiaMatrixDense(
+ Scalar mass, const Vec3& cogPosition, const Mat33& tensor)
+: Base()
+{
+ setTheFixedZeros();
+ set(mass, cogPosition, tensor);
+}
+
+TPL
+inline void CLASS::fill(Scalar mass, const Vec3& comPosition,
+ const Mat33& tensor)
+{
+ set(mass, comPosition, tensor);
+}
+
+TPL
+inline typename CLASS::Scalar CLASS::getMass() const
+{
+ return data(LX,LX);
+}
+
+TPL
+inline typename CLASS::Vec3 CLASS::getCOM() const
+{
+ return Vec3(
+ data(AZ,LY)/data(LX,LX), // X coordinate of the COM
+ data(AX,LZ)/data(LX,LX), // Y
+ data(AY,LX)/data(LX,LX));// Z
+}
+
+TPL
+inline const typename CLASS::Block33_const CLASS::get3x3Tensor() const
+{
+ return block33(AX,AX);
+}
+
+TPL
+inline void CLASS::changeMass(Scalar newMass) {
+ // Note the use of indices AX and hard-coded 0, to make it independent from
+ // the convention angular/linear
+ this->block<3,6>(AX,0) *= newMass/getMass();
+ block33(LX,AX) = block33(AX,LX).transpose();
+ data(LX,LX) = data(LY,LY) = data(LZ,LZ) = newMass;
+}
+
+TPL
+inline void CLASS::changeCOM(const Vec3& newcom)
+{
+ // Update the angular-linear block according to the new COM position
+ setSkewSymmetricBlock( getMass() * newcom, block33(AX,LX));
+
+ // Correct the 3x3 tensor. Use the block(AX,LX) which reflects the new COM
+ // and the block(LX,AX) which instead is still based on the previous value.
+ // I = I - m(cx)(cx)^T + m(c'x)(c'x)^T
+ // where cx is the skew symmetric matrix for the old COM vector, while
+ // c'x is the one for the new COM vector.
+ block33(AX,AX) += (
+ ( block33(AX,LX) * block33(AX,LX).transpose() ) -
+ ( block33(LX,AX).transpose() * block33(LX,AX) )
+ ) / getMass();
+ // Update the linear-angular block
+ block33(LX,AX) = block33(AX,LX).transpose();
+ // alternatively:
+ // setSkewSymmetricBlock( - getMass() * newcom, block33(LX,AX));
+}
+
+TPL
+inline void CLASS::changeRotationalInertia(const Mat33& tensor)
+{
+ block33(AX,AX) = tensor;
+}
+
+TPL
+inline void CLASS::set(
+ Scalar mass,
+ const Vec3& com,
+ const Mat33& tensor)
+{
+ block33(AX,AX) = tensor;// + mass * comCrossMx * comCrossMx.transpose();
+
+ data(AX,LY) = data(LY,AX) = - ( data(AY,LX) = data(LX,AY) = mass*com(Z) );
+ data(AZ,LX) = data(LX,AZ) = - ( data(AX,LZ) = data(LZ,AX) = mass*com(Y) );
+ data(AY,LZ) = data(LZ,AY) = - ( data(AZ,LY) = data(LY,AZ) = mass*com(X) );
+ // Equivalent, just slightly less efficient (probably because of Eigen's blocks and repeated product):
+ //setSkewSymmetricBlock( mass * com, block33(AX,LX));
+ //setSkewSymmetricBlock(- mass * com, block33(LX,AX));
+
+ data(LX,LX) = data(LY,LY) = data(LZ,LZ) = mass;
+}
+
+TPL
+template<typename OtherDerived>
+inline CLASS& CLASS::operator=
+ (const MatrixBase<OtherDerived>& other)
+{
+ // Here we silently assume that 'other' is also an inertia...
+ // Type safety would suggest to prevent the assignment of anything but
+ // another inertia, but that would prevent, for example, the assignment
+ // of matrix expressions. We also do not want to perform any check, for
+ // performance reasons (remember this library is meant primarily to support
+ // code generation, not to be a robust API for user applications).
+ block33(AX,AX) = other.template block<3,3>(AX,AX);
+ data(AX,LY) = data(LY,AX) = - ( data(AY,LX) = data(LX,AY) = other(LX,AY) );
+ data(AZ,LX) = data(LX,AZ) = - ( data(AX,LZ) = data(LZ,AX) = other(LZ,AX) );
+ data(AY,LZ) = data(LZ,AY) = - ( data(AZ,LY) = data(LY,AZ) = other(LY,AZ) );
+ data(LX,LX) = data(LY,LY) = data(LZ,LZ) = other(LX,LX);
+ return *this;
+}
+
+TPL
+template<typename OtherDerived>
+inline CLASS& CLASS::operator+=
+ (const MatrixBase<OtherDerived>& other)
+{
+ block33(AX,AX) += other.template block<3,3>(AX,AX);
+ data(AX,LY) = data(LY,AX) = - ( data(AY,LX) = (data(LX,AY) += other(LX,AY)) );
+ data(AZ,LX) = data(LX,AZ) = - ( data(AX,LZ) = (data(LZ,AX) += other(LZ,AX)) );
+ data(AY,LZ) = data(LZ,AY) = - ( data(AZ,LY) = (data(LY,AZ) += other(LY,AZ)) );
+ data(LX,LX) = data(LY,LY) = (data(LZ,LZ) += other(LX,LX));
+ return *this;
+}
+
+TPL
+inline void CLASS::setTheFixedZeros()
+{
+ block33(LX,LX).setZero(); // the diagonal won't be zero, but who cares
+ data(AX,LX) = data(AY,LY) = data(AZ,LZ) =
+ data(LX,AX) = data(LY,AY) = data(LZ,AZ) = 0;
+}
+
+TPL
+template<typename Vector>
+inline void CLASS::setSkewSymmetricBlock(
+ const MatrixBase<Vector>& v, Block33_t block)
+{
+ block(X,Y) = - ( block(Y,X) = v(Z) );
+ block(Z,X) = - ( block(X,Z) = v(Y) );
+ block(Y,Z) = - ( block(Z,Y) = v(X) );
+}
+
+
+#undef block33
+#undef data
+#undef TPL
+#undef CLASS
+}
+
+using InertiaMatrixDense = tpl::InertiaMatrixDense<double>;
+
+}
+}
+
+
+#endif /* INERTIAMATRIX_H_ */
diff --git a/ct_rbd/include/iit/rbd/StateDependentBase.h b/ct_rbd/include/iit/rbd/StateDependentBase.h
new file mode 100644
index 0000000..34e2b53
--- /dev/null
+++ b/ct_rbd/include/iit/rbd/StateDependentBase.h
@@ -0,0 +1,56 @@
+/* CPYHDR { */
+/*
+ * This file is part of the 'iit-rbd' library.
+ * Copyright © 2015 2016, Marco Frigerio (marco.frigerio@iit.it)
+ *
+ * See the LICENSE file for more information.
+ */
+/* } CPYHDR */
+
+#ifndef _IIT_RBD__STATE_DEPENDENT_BASE_H_
+#define _IIT_RBD__STATE_DEPENDENT_BASE_H_
+
+#include <iostream>
+#include "rbd.h"
+
+namespace iit {
+namespace rbd {
+
+/**
+ * A sort of minimal interface to model the dependency on some kind of state
+ * variable.
+ * The type of such a state variable is the first template parameter.
+ *
+ * The second parameter must be a class inheriting from this one, i.e., this
+ * class is supposed to be used according to the "curiously recurring template
+ * pattern".
+ * The sub-class shall implement the update() function with the actual,
+ * class-specific logic to update the instance, because the implementation
+ * of the operator(State const&) relies on update(State const&).
+ */
+template<class State, class Actual>
+class StateDependentBase
+{
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ /**
+ * Updates this object according to the given state.
+ * The actual logic of the update must be implemented in the update()
+ * function.
+ */
+ const Actual& operator()(State const& state) {
+ return static_cast<Actual*>(this) -> update(state);
+ }
+ /**
+ * Updates this object according to the given state variable.
+ * Subclasses shall implement this method since the implementation of
+ * operator()(State const&) relies on it.
+ */
+ const Actual& update(State const& state);
+};
+
+}
+}
+
+
+#endif
diff --git a/ct_rbd/include/iit/rbd/StateDependentMatrix.h b/ct_rbd/include/iit/rbd/StateDependentMatrix.h
new file mode 100644
index 0000000..a0abd1e
--- /dev/null
+++ b/ct_rbd/include/iit/rbd/StateDependentMatrix.h
@@ -0,0 +1,70 @@
+/* CPYHDR { */
+/*
+ * This file is part of the 'iit-rbd' library.
+ * Copyright © 2015 2016, Marco Frigerio (marco.frigerio@iit.it)
+ *
+ * See the LICENSE file for more information.
+ */
+/* } CPYHDR */
+
+#ifndef _IIT_RBD__STATE_DEPENDENT_MATRIX_H_
+#define _IIT_RBD__STATE_DEPENDENT_MATRIX_H_
+
+#include <iostream>
+
+#include "rbd.h"
+#include "StateDependentBase.h"
+
+namespace iit {
+namespace rbd {
+
+
+/**
+ * A matrix that exposes a dependency on some kind of state variable.
+ * The type of such a variable is the first template parameter.
+ * The second and third parameters are the number of rows and columns.
+ *
+ * The last parameter must be a class inheriting from this class, i.e., we
+ * are using here the curiously recurring template pattern.
+ * Such a sub-class shall implement the update() function. See the documentation
+ * of StateDependentBase.
+ *
+ * This class was created explicitly to support code generation
+ * e.g. of coordinate transforms.
+ */
+template<class State, int Rows, int Cols, class ActualMatrix>
+class StateDependentMatrix :
+ public StateDependentBase<State, ActualMatrix>,
+ public PlainMatrix<typename State::Scalar, Rows, Cols>
+{
+ private:
+ typedef PlainMatrix<typename State::Scalar, Rows, Cols> Base;
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ /** The type of the coefficients of this matrix */
+ typedef typename Base::Scalar Scalar;
+ /** The type of row/column indices of this matrix */
+ typedef typename Base::Index Index;
+ /** The regular matrix type this class inherits from */
+ typedef Base MatrixType;
+ public:
+ StateDependentMatrix() {};
+ ~StateDependentMatrix() {}
+
+ template<typename OtherDerived>
+ StateDependentMatrix& operator= (const MatrixBase<OtherDerived>& other) {
+ this->Base::operator=(other);
+ return *this;
+ }
+
+ using StateDependentBase<State, ActualMatrix>::operator();
+ using MatrixType::operator();
+ using MatrixType::setZero;
+};
+
+
+}
+}
+
+
+#endif /* _IIT_RBD__STATE_DEPENDENT_MATRIX_H_ */
diff --git a/ct_rbd/include/iit/rbd/TransformsBase.h b/ct_rbd/include/iit/rbd/TransformsBase.h
new file mode 100644
index 0000000..8d0bfe6
--- /dev/null
+++ b/ct_rbd/include/iit/rbd/TransformsBase.h
@@ -0,0 +1,75 @@
+/* CPYHDR { */
+/*
+ * This file is part of the 'iit-rbd' library.
+ * Copyright © 2015 2016, Marco Frigerio (marco.frigerio@iit.it)
+ *
+ * See the LICENSE file for more information.
+ */
+/* } CPYHDR */
+
+#ifndef IIT_RBD_TRANSFORMSBASE_H_
+#define IIT_RBD_TRANSFORMSBASE_H_
+
+#include "StateDependentMatrix.h"
+
+namespace iit {
+namespace rbd {
+
+
+/**
+ * A 3x3 specialization of StateDependentMatrix, to be used as a base class for
+ * rotation matrices that depend on a state variable.
+ */
+template<class State, class ActualMatrix>
+class RotationTransformBase : public StateDependentMatrix<State, 3, 3, ActualMatrix>
+{
+public:
+ RotationTransformBase() : StateDependentMatrix<State, 3, 3, ActualMatrix>() {}
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+/**
+ * A 4x4 specialization of StateDependentMatrix, to be used as a base class for
+ * homogeneous transformation matrices that depend on a state variable.
+ */
+template<class State, class ActualMatrix>
+class HomogeneousTransformBase : public StateDependentMatrix<State, 4, 4, ActualMatrix>
+{
+public:
+ HomogeneousTransformBase() : StateDependentMatrix<State, 4, 4, ActualMatrix>() {}
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+/**
+ * A 6x6 specialization of StateDependentMatrix, to be used as a base class for
+ * spatial transformation matrices that depend on a state variable.
+ */
+template<class State, class ActualMatrix>
+class SpatialTransformBase : public StateDependentMatrix<State, 6, 6, ActualMatrix>
+{
+public:
+ SpatialTransformBase() : StateDependentMatrix<State, 6, 6, ActualMatrix>() {}
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+/**
+ * A 6xCols specialization of StateDependentMatrix, to be used as a base class
+ * for geometric Jacobians that depend on a state variable.
+ */
+template<class State, int Cols, class ActualMatrix>
+class JacobianBase : public StateDependentMatrix<State, 6, Cols, ActualMatrix>
+{
+public:
+ JacobianBase() : StateDependentMatrix<State, 6, Cols, ActualMatrix>() {}
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+
+
+
+}
+}
+
+
+
+#endif /* IIT_RBD_TRANSFORMSBASE_H_ */
diff --git a/ct_rbd/include/iit/rbd/eigen_traits.h b/ct_rbd/include/iit/rbd/eigen_traits.h
new file mode 100644
index 0000000..0c65192
--- /dev/null
+++ b/ct_rbd/include/iit/rbd/eigen_traits.h
@@ -0,0 +1,80 @@
+/* CPYHDR { */
+/*
+ * This file is part of the 'iit-rbd' library.
+ * Copyright © 2015 2016, Marco Frigerio (marco.frigerio@iit.it)
+ *
+ * See the LICENSE file for more information.
+ */
+/* } CPYHDR */
+
+#ifndef IIT_RBD_EIGEN_TRAITS_
+#define IIT_RBD_EIGEN_TRAITS_
+
+
+#include "StateDependentMatrix.h"
+#include "TransformsBase.h"
+
+/**
+ * \file
+ * This header file contains some instantiations of the \c traits template (in
+ * the \c Eigen::internal namespace) for matrix types defined in \c iit::rbd
+ */
+
+namespace Eigen {
+namespace internal {
+
+/**
+ * The Eigen traits for the iit::rbd::HomogeneousTransformBase type
+ */
+template<typename State, typename M>
+struct traits< iit::rbd::HomogeneousTransformBase<State, M> >
+{
+ typedef typename iit::rbd::HomogeneousTransformBase<State, M>::MatrixType MxType;
+ typedef traits<MxType> Traits;
+ typedef typename Traits::Scalar Scalar;
+ typedef typename Traits::StorageKind StorageKind;
+ typedef typename Traits::Index Index;
+ typedef typename Traits::XprKind XprKind;
+ enum {
+ RowsAtCompileTime = Traits::RowsAtCompileTime,
+ ColsAtCompileTime = Traits::ColsAtCompileTime,
+ MaxRowsAtCompileTime = Traits::MaxRowsAtCompileTime,
+ MaxColsAtCompileTime = Traits::MaxColsAtCompileTime,
+ Options = Traits::Options,
+ Flags = Traits::Flags,
+ CoeffReadCost = Traits::CoeffReadCost,
+ InnerStrideAtCompileTime = Traits::InnerStrideAtCompileTime,
+ OuterStrideAtCompileTime = Traits::OuterStrideAtCompileTime
+ };
+};
+
+/**
+ * The Eigen traits for the iit::rbd::RotationTransformBase type
+ */
+template<typename State, typename M>
+struct traits< iit::rbd::RotationTransformBase<State, M> >
+{
+ typedef typename iit::rbd::RotationTransformBase<State, M>::MatrixType MxType;
+ typedef traits<MxType> Traits;
+ typedef typename Traits::Scalar Scalar;
+ typedef typename Traits::StorageKind StorageKind;
+ typedef typename Traits::Index Index;
+ typedef typename Traits::XprKind XprKind;
+ enum {
+ RowsAtCompileTime = Traits::RowsAtCompileTime,
+ ColsAtCompileTime = Traits::ColsAtCompileTime,
+ MaxRowsAtCompileTime = Traits::MaxRowsAtCompileTime,
+ MaxColsAtCompileTime = Traits::MaxColsAtCompileTime,
+ Options = Traits::Options,
+ Flags = Traits::Flags,
+ CoeffReadCost = Traits::CoeffReadCost,
+ InnerStrideAtCompileTime = Traits::InnerStrideAtCompileTime,
+ OuterStrideAtCompileTime = Traits::OuterStrideAtCompileTime
+ };
+};
+
+
+}
+}
+
+#endif
diff --git a/ct_rbd/include/iit/rbd/internals.h b/ct_rbd/include/iit/rbd/internals.h
new file mode 100644
index 0000000..fe7aa20
--- /dev/null
+++ b/ct_rbd/include/iit/rbd/internals.h
@@ -0,0 +1,187 @@
+/* CPYHDR { */
+/*
+ * This file is part of the 'iit-rbd' library.
+ * Copyright © 2015 2016, Marco Frigerio (marco.frigerio@iit.it)
+ *
+ * See the LICENSE file for more information.
+ */
+/* } CPYHDR */
+
+#ifndef IIT_RBD_INTERNALS_
+#define IIT_RBD_INTERNALS_
+
+#include "InertiaMatrix.h"
+
+namespace iit {
+namespace rbd {
+namespace internal {
+
+/*
+ * A container of the 9 coefficients of a 3x3 matrix.
+ *
+ * When multiple, repeated access to the elements of the matrix is necessary,
+ * copying them once at the beginning in local variables might slightly improve
+ * the performance (coefficient access typically comes at the cost of few
+ * arithmetic operations)
+ */
+template<typename Scalar>
+struct Mat3x3Coefficients
+{
+ Scalar XX,XY,XZ,YX,YY,YZ,ZX,ZY,ZZ;
+
+ Mat3x3Coefficients() {}
+ Mat3x3Coefficients(Scalar xx, Scalar xy, Scalar xz,
+ Scalar yx, Scalar yy, Scalar yz,
+ Scalar zx, Scalar zy, Scalar zz) :
+ XX(xx), XY(xy), XZ(xz),
+ YX(yx), YY(yy), YZ(yz),
+ ZX(zx), ZY(zy), ZZ(zz)
+ {}
+
+ template <typename D>
+ Mat3x3Coefficients(const MatrixBase<D>& E) :
+ XX(E(X,X)), XY(E(X,Y)), XZ(E(X,Z)),
+ YX(E(Y,X)), YY(E(Y,Y)), YZ(E(Y,Z)),
+ ZX(E(Z,X)), ZY(E(Z,Y)), ZZ(E(Z,Z))
+ {}
+
+ template <typename D>
+ void read(const MatrixBase<D>& E)
+ {
+ XX = E(X,X); XY = E(X,Y); XZ = E(X,Z);
+ YX = E(Y,X); YY = E(Y,Y); YZ = E(Y,Z);
+ ZX = E(Z,X); ZY = E(Z,Y); ZZ = E(Z,Z);
+ }
+};
+
+/*
+ * A container of the 6 distinct coefficients of a symmetrix 3x3 matrix.
+ *
+ * See comment above.
+ */
+template<typename Scalar>
+struct SymmMat3x3Coefficients {
+ Scalar XX,XY,XZ,YY,YZ,ZZ;
+
+ SymmMat3x3Coefficients() {}
+
+ SymmMat3x3Coefficients(Scalar xx, Scalar xy, Scalar xz,
+ Scalar yy, Scalar yz,
+ Scalar zz) :
+ XX(xx), XY(xy), XZ(xz),
+ YY(yy), YZ(yz),
+ ZZ(zz)
+ {}
+
+ template <typename D>
+ SymmMat3x3Coefficients(const MatrixBase<D>& E) {
+ read(E);
+ }
+
+ template <typename D>
+ void read(const MatrixBase<D>& E)
+ {
+ XX = E(X,X); XY = E(X,Y); XZ = E(X,Z);
+ YY = E(Y,Y); YZ = E(Y,Z);
+ ZZ = E(Z,Z);
+ }
+ template <typename D>
+ void write(const MatrixBase<D>& Econst)
+ {
+ MatrixBase<D>& E = const_cast<MatrixBase<D>&>(Econst);
+ E(X,X) = XX; E(X,Y) = XY; E(X,Z) = XZ;
+ E(Y,X) = XY; E(Y,Y) = YY; E(Y,Z) = YZ;
+ E(Z,X) = XZ; E(Z,Y) = YZ; E(Z,Z) = ZZ;
+ }
+};
+
+/*
+ * Performs the transformation of a symmetrix 3x3 matrix A, with the rotation
+ * matrix E
+ * B = E * A * E^T
+ *
+ * These calculations are documented in the appendix of Roy's book
+ */
+template<typename Scalar>
+inline void rot_symmetric_EAET(
+ const Mat3x3Coefficients<Scalar>& E,
+ const SymmMat3x3Coefficients<Scalar>& A,
+ SymmMat3x3Coefficients<Scalar>& B)
+{
+ Scalar LXX = A.XX - A.ZZ;
+ Scalar LXY = A.XY; // same as LYX
+ Scalar LYY = A.YY - A.ZZ;
+ Scalar LZX = 2*A.XZ;
+ Scalar LZY = 2*A.YZ;
+
+ Scalar yXX = E.YX*LXX + E.YY*LXY + E.YZ*LZX;
+ Scalar yXY = E.YX*LXY + E.YY*LYY + E.YZ*LZY;
+ Scalar yYX = E.ZX*LXX + E.ZY*LXY + E.ZZ*LZX;
+ Scalar yYY = E.ZX*LXY + E.ZY*LYY + E.ZZ*LZY;
+
+ Scalar v1 = -A.YZ;
+ Scalar v2 = A.XZ;
+ Scalar EvX = E.XX*v1 + E.XY*v2;
+ Scalar EvY = E.YX*v1 + E.YY*v2;
+ Scalar EvZ = E.ZX*v1 + E.ZY*v2;
+
+ B.XY = yXX * E.XX + yXY * E.XY + EvZ;
+ B.XZ = yYX * E.XX + yYY * E.XY - EvY;
+ B.YZ = yYX * E.YX + yYY * E.YY + EvX;
+
+ Scalar zYY = yXX * E.YX + yXY * E.YY;
+ Scalar zZZ = yYX * E.ZX + yYY * E.ZY;
+ B.XX = LXX + LYY - zYY - zZZ + A.ZZ;
+ B.YY = zYY + A.ZZ;
+ B.ZZ = zZZ + A.ZZ;
+}
+
+/*
+ * Performs the transformation of a 3x3 matrix A, with the rotation
+ * matrix E
+ * B = E * A * E^T
+ */
+template<typename Scalar>
+inline void rot_EAET(
+ const Mat3x3Coefficients<Scalar>& E,
+ const Mat3x3Coefficients<Scalar>& A,
+ Mat3x3Coefficients<Scalar>& B)
+{
+ Scalar LXX = A.XX - A.ZZ;
+ Scalar LXY = A.XY;
+ Scalar LYX = A.YX;
+ Scalar LYY = A.YY - A.ZZ;
+ Scalar LZX = A.ZX + A.XZ;
+ Scalar LZY = A.ZY + A.YZ;
+
+ Scalar v1 = -A.YZ;
+ Scalar v2 = A.XZ;
+ Scalar EvX = E.XX*v1 + E.XY*v2;
+ Scalar EvY = E.YX*v1 + E.YY*v2;
+ Scalar EvZ = E.ZX*v1 + E.ZY*v2;
+
+ Scalar yXX = E.XX*LXX + E.XY*LYX + E.XZ*LZX;
+ Scalar yXY = E.XX*LXY + E.XY*LYY + E.XZ*LZY;
+ Scalar yYX = E.YX*LXX + E.YY*LYX + E.YZ*LZX;
+ Scalar yYY = E.YX*LXY + E.YY*LYY + E.YZ*LZY;
+ Scalar yZX = E.ZX*LXX + E.ZY*LYX + E.ZZ*LZX;
+ Scalar yZY = E.ZX*LXY + E.ZY*LYY + E.ZZ*LZY;
+
+ B.XX = yXX*E.XX + yXY*E.XY + A.ZZ;
+ B.YY = yYX*E.YX + yYY*E.YY + A.ZZ;
+ B.ZZ = yZX*E.ZX + yZY*E.ZY + A.ZZ;
+
+ B.XY = yXX*E.YX + yXY*E.YY - EvZ;
+ B.YX = yYX*E.XX + yYY*E.XY + EvZ;
+ B.XZ = yXX*E.ZX + yXY*E.ZY + EvY;
+ B.ZX = yZX*E.XX + yZY*E.XY - EvY;
+ B.YZ = yYX*E.ZX + yYY*E.ZY - EvX;
+ B.ZY = yZX*E.YX + yZY*E.YY + EvX;
+}
+
+
+}
+}
+}
+
+#endif
diff --git a/ct_rbd/include/iit/rbd/rbd.h b/ct_rbd/include/iit/rbd/rbd.h
new file mode 100644
index 0000000..8878010
--- /dev/null
+++ b/ct_rbd/include/iit/rbd/rbd.h
@@ -0,0 +1,191 @@
+/* CPYHDR { */
+/*
+ * This file is part of the 'iit-rbd' library.
+ * Copyright © 2015 2016, Marco Frigerio (marco.frigerio@iit.it)
+ *
+ * See the LICENSE file for more information.
+ */
+/* } CPYHDR */
+#ifndef IIT_RBD_H_
+#define IIT_RBD_H_
+
+#include <Eigen/Dense>
+#include <Eigen/StdVector>
+#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET 1
+#include <Eigen/Sparse>
+
+
+namespace iit {
+/**
+ * This namespace contains some basic types and functions related to spatial
+ * vectors and Rigid Body Dynamics (RBD).
+ *
+ * This code has been developed to provide basic types and general utilities to
+ * be used in turn by the C++ classes generated by the Robotics Code Generator
+ * (RobCoGen). That is, the code is just meant to be a common, generic facility
+ * for all the robot-specific code that RobCoGen can generate.
+ *
+ * In other words, the primary target of the \c iit_rbd library is not to
+ * provide a comprehensive object-oriented implementation of concepts of
+ * rigid-body dynamics and spatial vectors, for end-user applications. Some
+ * of such concepts do appear in this software, although the implementation is
+ * relatively simple.
+ * However, the library may very well be used in custom code that needs e.g.
+ * a type for spatial vectors or for the spatial 6x6 inertia tensor.
+ */
+namespace rbd {
+
+/**
+ * \defgroup rbd-core RBD Core
+ * Main, basic data types and functions
+ * @{
+ */
+
+/**
+ * \name General matrix types
+ */
+///@{
+// Use alias templates to limit the references to 'Eigen' to this header
+
+template<typename Derived>
+using MatrixBase = Eigen::MatrixBase< Derived >;
+
+template<typename Scalar, int R, int C>
+using PlainMatrix = Eigen::Matrix< Scalar, R, C >;
+
+template<typename XprType, int R, int C>
+using MatrixBlock = Eigen::Block< XprType, R, C >;
+
+template<typename Scalar>
+using SparseMatrix = Eigen::SparseMatrix<Scalar>;
+
+template<typename Scalar>
+using SparseVector = Eigen::SparseVector<Scalar>;
+///@}
+
+/**
+ * A container of basic type/function definitions, templated on the scalar type.
+ *
+ * \tparam SCALAR the numerical type for scalar values
+ */
+template<typename SCALAR>
+struct Core
+{
+ typedef SCALAR Scalar;
+
+ /** \name Basic matrix types */
+ ///@{
+ typedef PlainMatrix<Scalar,3,3> Matrix33;
+ typedef PlainMatrix<Scalar,6,6> Matrix66;
+ typedef PlainMatrix<Scalar,3,1> Vector3;
+ typedef PlainMatrix<Scalar,6,1> Vector6;
+ ///@}
+
+ /**
+ * \name 6D vectors "à la Featherstone"
+ * Types of vectors used in dynamics computations.
+ */
+ ///@{
+ typedef Vector6 Vector6D; // here the 'D' stands for Dimension, not double !
+ typedef Vector6D Column6D;
+ typedef Vector6D VelocityVector;
+ typedef Vector6D ForceVector;
+
+ typedef MatrixBlock<Vector6D,3,1> Part3D; ///< a 3D subvector of a 6D vector
+ typedef MatrixBlock<const Vector6D,3,1> Part3DConst;///< a const 3D subvector of a 6D vector
+ ///@}
+
+
+ /**
+ * \name 6D vectors accessors
+ * These functions allow to access the linear and the angular
+ * coordinates of motion/force vectors.
+ */
+ ///@{
+ /**
+ * The 3-coordinate vector with the angular components (angular
+ * velocity or torque) of the given 6D vector.
+ */
+ static inline Part3D angularPart(Vector6D& f) {
+ return f.template topRows<3>();
+ }
+ /**
+ * The 3-coordinate vector with the linear components (linear
+ * velocity or force) of the given 6D vector.
+ */
+ static inline Part3D linearPart(Vector6D& f) {
+ return f.template bottomRows<3>();
+ }
+ static inline Part3DConst angularPart(const Vector6D& f) {
+ return f.template topRows<3>();
+ }
+ static inline Part3DConst linearPart(const Vector6D& f) {
+ return f.template bottomRows<3>();
+ }
+};
+
+
+/**
+ * \name Types with 'double' as the scalar type.
+ * Explicit instantiation of all the various matrix types using the standard
+ * \c double as the scalar type. See Core
+ */
+///@{
+typedef Core<double> Cored;
+
+using Matrix33d = Cored::Matrix33;
+using Vector3d = Cored::Vector3;
+using Matrix66d = Cored::Matrix66;
+using Vector6d = Cored::Vector6;
+
+using Vector6D = Cored::Vector6D;
+using VelocityVector = Cored::VelocityVector;
+using ForceVector = Cored::ForceVector;
+
+using Part3D = Cored::Part3D;
+using Part3DConst = Cored::Part3DConst;
+
+using Column6d = Cored::Column6D; // an alias
+
+using SparseMatrixd = SparseMatrix<double>;
+using SparseColumnd = SparseVector<double>;
+
+
+inline Part3D angularPart(Vector6D& f) {
+ return Cored::angularPart(f);
+}
+inline Part3D linearPart(Vector6D& f) {
+ return Cored::linearPart(f);
+}
+inline Part3DConst angularPart(const Vector6D& f) {
+ return Cored::angularPart(f);
+}
+inline Part3DConst linearPart(const Vector6D& f) {
+ return Cored::linearPart(f);
+}
+///@}
+
+/**
+ * \name Vector coordinates
+ * Constants to index either 6D or 3D coordinate vectors.
+ */
+///@{
+enum Coords3D { X=0, Y, Z};
+/// To be used with 6D vectors. 'A' stands for angular, 'L' for linear.
+enum Coords6D { AX=0, AY, AZ, LX, LY, LZ };
+///@}
+
+///@} // end of implicit in-group elements [Doxygen]
+
+/**
+ * The Earth gravitational acceleration constant.
+ * This value should always be positive, so that an application
+ * chooses the sign according to its conventions.
+ */
+static const double g = 9.81;
+
+}
+}
+
+
+#endif /* IIT_RBD_H_ */
diff --git a/ct_rbd/include/iit/rbd/robcogen_commons.h b/ct_rbd/include/iit/rbd/robcogen_commons.h
new file mode 100644
index 0000000..a264b1e
--- /dev/null
+++ b/ct_rbd/include/iit/rbd/robcogen_commons.h
@@ -0,0 +1,691 @@
+/* CPYHDR { */
+/*
+ * This file is part of the 'iit-rbd' library.
+ * Copyright © 2015 2016, Marco Frigerio (marco.frigerio@iit.it)
+ *
+ * See the LICENSE file for more information.
+ */
+/* } CPYHDR */
+
+#ifndef IIT_RBD_ROBCOGEN_COMMON_EXPRESSIONS_
+#define IIT_RBD_ROBCOGEN_COMMON_EXPRESSIONS_
+
+
+#include "rbd.h"
+#include "InertiaMatrix.h"
+#include "TransformsBase.h"
+#include "types.h"
+#include "internals.h"
+
+
+namespace iit {
+namespace rbd {
+
+/**
+ * \defgroup robcogen_commons RobCoGen commons
+ *
+ * Functions implementing some algebraic expressions recurring in the code
+ * generated by RobCoGen.
+ *
+ * This implementation is usually a bit more efficient than the straightforward
+ * way of doing the same thing, with matrix algebra.
+ *
+ * These functions were written specifically to be used within the code
+ * generated by RobCoGen; they expect parameters which have to satisfy maybe more
+ * constraints than one would expect from a public API. Similarly, no
+ * consistency checks are performed (e.g. verify that the matrix is 6x6).
+ * @{
+ */
+
+
+#define DScalar typename Derived::Scalar
+
+
+/**
+ * Calculates the bias force acting on a rigid body with the given velocity and
+ * inertial properties.
+ *
+ * This function provides a hopefully efficient implementation of the term
+ * \f[ v \times^{*} I v \f]
+ * as described in Featherstone's book about rigid body dynamics algorithms.
+ *
+ * \param v the spatial velocity of the body
+ * \param I the spatial inertia tensor of the body
+ * \return the spatial force acting on the body due to its own velocity; from
+ * another point of view, the force required not to produce any acceleration.
+ */
+template <typename Derived>
+Force<DScalar> vxIv(const Velocity<DScalar>& v, const MatrixBase<Derived>& I)
+{
+ typedef DScalar Scalar;
+ // This is a "manual" implementation of the matrix product, as the obvious
+ // implementation (plain matrix-matrix-vector multiplication) would not
+ // exploit the (known) sparsity pattern and the fact that the first and last
+ // term ( v cross* and v) have the same coefficients.
+
+ // Pre-compute some recurring quantities.
+ Scalar wx2 = v(AX)*v(AX);
+ Scalar wy2 = v(AY)*v(AY);
+ Scalar wz2 = v(AZ)*v(AZ);
+
+ Scalar v_AXAY = v(AX)*v(AY);
+ Scalar v_AXAZ = v(AX)*v(AZ);
+ Scalar v_AXLY = v(AX)*v(LY);
+ Scalar v_AXLZ = v(AX)*v(LZ);
+
+ Scalar v_AYAZ = v(AY)*v(AZ);
+ Scalar v_AYLX = v(AY)*v(LX);
+ Scalar v_AYLZ = v(AY)*v(LZ);
+
+ Scalar v_AZLX = v(AZ)*v(LX);
+ Scalar v_AZLY = v(AZ)*v(LY);
+
+ Scalar m = I(LX,LX); // the mass
+ // Copy some of the coefficients for more efficient access
+ // 'aa' stands for angular-angular, a 3x3 sub-block of the spatial inertia
+ internal::SymmMat3x3Coefficients<Scalar> Iaa(
+ I(AX,AX), I(AX,AY), I(AX,AZ),
+ I(AY,AY), I(AY,AZ),
+ I(AZ,AZ));
+ Scalar cmX = I(AZ, LY);
+ Scalar cmY = I(AX, LZ);
+ Scalar cmZ = I(AY, LX);
+
+ typename Core<Scalar>::ForceVector ret;
+ ret(AX) = cmZ * (v_AXLZ - v_AZLX)
+ + cmY * (v_AXLY - v_AYLX)
+ + v_AYAZ * (Iaa.ZZ - Iaa.YY)
+ - v_AXAZ*Iaa.XY - (wz2 - wy2)*Iaa.YZ + v_AXAY*Iaa.XZ;
+
+ ret(AY) = cmZ * (v_AYLZ - v_AZLY)
+ - cmX * (v_AXLY - v_AYLX)
+ - v_AXAZ * (Iaa.ZZ - Iaa.XX)
+ + v_AYAZ*Iaa.XY - v_AXAY*Iaa.YZ + (wz2 - wx2)*Iaa.XZ;
+
+ ret(AZ) = cmX * (v_AZLX - v_AXLZ)
+ - cmY * (v_AYLZ - v_AZLY)
+ + v_AXAY * (Iaa.YY - Iaa.XX)
+ + v_AXAZ*Iaa.YZ - Iaa.XZ*v_AYAZ - (wy2 - wx2)*Iaa.XY;
+
+ ret(LX) = m * (v_AYLZ - v_AZLY) - cmX * (wz2 + wy2) + v_AXAZ*cmZ + v_AXAY*cmY;
+ ret(LY) = - m * (v_AXLZ - v_AZLX) - cmY * (wz2 + wx2) + v_AXAY*cmX + v_AYAZ*cmZ;
+ ret(LZ) = m * (v_AXLY - v_AYLX) - cmZ * (wy2 + wx2) + v_AXAZ*cmX + v_AYAZ*cmY;
+ return ret;
+}
+
+/**
+ * Calculates the bias force in the special case in which the velocity is a pure
+ * rotation about the \c z axis.
+ * \see vxIv(const VelocityVector&, const InertiaMatrixDense&)
+ *
+ * \param omegaz the scalar rotation velocity about the \c z axis
+ * \param I the spatial inertia tensor of the rigid body
+ * \return see vxIv(const VelocityVector&, const InertiaMatrixDense&)
+ */
+template <typename Derived>
+inline Force<DScalar> vxIv(const DScalar& omegaz, const MatrixBase<Derived>& I)
+{
+ DScalar wz2 = omegaz*omegaz;
+ Force<DScalar> ret;
+ ret(AX) = -I(AY,AZ) * wz2;
+ ret(AY) = I(AX,AY) * wz2;
+ ret(AZ) = 0;
+ ret(LX) = I(AY,LZ) * wz2;
+ ret(LY) = I(AZ,LX) * wz2;
+ ret(LZ) = 0;
+ return ret;
+}
+
+// very important, un-define the convenience macros to avoid polluting the namespace
+#undef DScalar
+
+
+/**
+ * Calculates the spatial cross product matrix, for motion vectors.
+ *
+ * \param v the spatial velocity defining the cross product
+ * \param[out] vx the 6x6 matrix whose coefficients will be set; must be
+ * initialized with zeros (that is, this function does not assign the
+ * zero elements of the cross product matrix).
+ * \tparam the scalar type
+ */
+template<typename S = double>
+inline void motionCrossProductMx(const Velocity<S>& v, Mat66<S>& vx)
+{
+ vx(AY,AZ) = vx(LY,LZ) = - ( vx(AZ,AY) = vx(LZ,LY) = v(AX) );
+ vx(AZ,AX) = vx(LZ,LX) = - ( vx(AX,AZ) = vx(LX,LZ) = v(AY) );
+ vx(AX,AY) = vx(LX,LY) = - ( vx(AY,AX) = vx(LY,LX) = v(AZ) );
+
+ vx(LY,AZ) = - ( vx(LZ,AY) = v(LX) );
+ vx(LZ,AX) = - ( vx(LX,AZ) = v(LY) );
+ vx(LX,AY) = - ( vx(LY,AX) = v(LZ) );
+}
+/**
+ * Calculates the spatial cross product matrix, for force vectors.
+ *
+ * \param v the spatial velocity defining the cross product
+ * \param[out] vx the 6x6 matrix whose coefficients will be set; must be
+ * initialized with zeros (that is, this function does not assign the
+ * zero elements of the cross product matrix).
+ */
+template<typename S = double>
+inline void forceCrossProductMx(const Velocity<S>& v, Mat66<S>& vx)
+{
+ vx(AY,AZ) = vx(LY,LZ) = - ( vx(AZ,AY) = vx(LZ,LY) = v(AX) );
+ vx(AZ,AX) = vx(LZ,LX) = - ( vx(AX,AZ) = vx(LX,LZ) = v(AY) );
+ vx(AX,AY) = vx(LX,LY) = - ( vx(AY,AX) = vx(LY,LX) = v(AZ) );
+
+ vx(AY,LZ) = - ( vx(AZ,LY) = v(LX) );
+ vx(AZ,LX) = - ( vx(AX,LZ) = v(LY) );
+ vx(AX,LY) = - ( vx(AY,LX) = v(LZ) );
+}
+
+
+template<typename S = double>
+void fillInertia(S mass, const Vec3<S>& com, const internal::SymmMat3x3Coefficients<S>& I3x3,
+ InertiaMat<S>& I)
+{
+ I(AX,AX) = I3x3.XX;
+ I(AY,AY) = I3x3.YY;
+ I(AZ,AZ) = I3x3.ZZ;
+ I(AY,AX) = I(AX,AY) = I3x3.XY;
+ I(AZ,AX) = I(AX,AZ) = I3x3.XZ;
+ I(AZ,AY) = I(AY,AZ) = I3x3.YZ;
+
+ I(AX,LY) = I(LY,AX) = - ( I(AY,LX) = I(LX,AY) = mass*com(Z) );
+ I(AZ,LX) = I(LX,AZ) = - ( I(AX,LZ) = I(LZ,AX) = mass*com(Y) );
+ I(AY,LZ) = I(LZ,AY) = - ( I(AZ,LY) = I(LY,AZ) = mass*com(X) );
+
+ I(LX,LX) = I(LY,LY) = I(LZ,LZ) = mass;
+}
+
+
+/**
+ * Roto-translate a spatial inertia tensor.
+ *
+ * This function is basically an optimized implementation of the formula to do
+ * a coordinate transform for a spatial inertia:
+ * \f[ XF \cdot I \cdot XF^T \f]
+ * (see equation 2.66 of the RBDA book)
+ *
+ * \param I_A the spatial inertia tensor of a rigid body, expressed in reference frame \c A
+ * \param XF a spatial coordinate transform for force vectors, in the form \c B_XF_A
+ * (that is, mapping forces from A to B coordinates)
+ * \param[out] I_B the output tensor, expressing the same inertial properties with
+ * coordinates of frame \c B
+ */
+template<typename Scalar = double>
+void transformInertia(
+ const InertiaMat<Scalar>& I_A,
+ const Mat66<Scalar>& XF,
+ InertiaMat<Scalar>& I_B)
+{
+ // The coefficients of the 3x3 rotation matrix
+ internal::Mat3x3Coefficients<Scalar> E(
+ XF(AX,AX),XF(AX,AY),XF(AX,AZ),
+ XF(AY,AX),XF(AY,AY),XF(AY,AZ),
+ XF(AZ,AX),XF(AZ,AY),XF(AZ,AZ));
+ Scalar mass = I_A.getMass();
+
+ // The relative position 'r' of the origin of frame B wrt A (in A coordinates).
+ // The vector is basically "reconstructed" from the matrix XF, which has
+ // this form:
+ // | E -E rx |
+ // | 0 E |
+ // where 'rx' is the cross product matrix. The strategy is to compute
+ // E^T (-E rx) = -rx , and then get the coordinates of 'r' from 'rx'.
+ // This code is a manual implementation of the E transpose multiplication,
+ // limited to the elements of interest.
+ // Note that this is necessary because, currently, spatial transforms do
+ // not carry explicitly the information about the translation vector.
+ Scalar rx = E.XY * XF(AX,LZ) + E.YY * XF(AY,LZ) + E.ZY * XF(AZ,LZ);
+ Scalar ry = E.XZ * XF(AX,LX) + E.YZ * XF(AY,LX) + E.ZZ * XF(AZ,LX);
+ Scalar rz = E.XX * XF(AX,LY) + E.YX * XF(AY,LY) + E.ZX * XF(AZ,LY);
+
+ // The relative position of the CoM wrt A (in A coordinates)
+ Scalar comAx = I_A(AZ,LY)/mass;
+ Scalar comAy = I_A(AX,LZ)/mass;
+ Scalar comAz = I_A(AY,LX)/mass;
+
+ // The relative position of the CoM wrt B (in A coordinates)
+ typename Core<Scalar>::Vector3 p(comAx-rx, comAy-ry, comAz-rz);
+
+ // Pre-computation of some recurring squares
+ Scalar cx2 = comAx*comAx;
+ Scalar cy2 = comAy*comAy;
+ Scalar cz2 = comAz*comAz;
+ Scalar px2 = p(X)*p(X);
+ Scalar py2 = p(Y)*p(Y);
+ Scalar pz2 = p(Z)*p(Z);
+
+ // Manual implementation of the parallel axis theorem, to translate the
+ // 3x3 tensor from frame A to frame B :
+ internal::SymmMat3x3Coefficients<Scalar> I_translated(
+ I_A(AX,AX), I_A(AX,AY), I_A(AX,AZ)
+ , I_A(AY,AY), I_A(AY,AZ)
+ , I_A(AZ,AZ));
+ I_translated.XX += mass*( py2 + pz2 - cy2 - cz2 );
+ I_translated.YY += mass*( px2 + pz2 - cx2 - cz2 );
+ I_translated.ZZ += mass*( px2 + py2 - cx2 - cy2 );
+ I_translated.XY += mass*( comAx*comAy - p(X)*p(Y) );
+ I_translated.XZ += mass*( comAx*comAz - p(X)*p(Z) );
+ I_translated.YZ += mass*( comAy*comAz - p(Y)*p(Z) );
+
+ // Rotate the 3x3 tensor
+ internal::SymmMat3x3Coefficients<Scalar> I3x3_B;
+ internal::rot_symmetric_EAET<Scalar>(E, I_translated, I3x3_B);
+ // Rotate the CoM vector
+ typename Core<Scalar>::Vector3 p_B(
+ E.XX*p(X) + E.XY*p(Y) + E.XZ*p(Z),
+ E.YX*p(X) + E.YY*p(Y) + E.YZ*p(Z),
+ E.ZX*p(X) + E.ZY*p(Y) + E.ZZ*p(Z)
+ );
+
+ // Finally copy the coefficients into the destination matrix
+ fillInertia(mass, p_B, I3x3_B, I_B);
+}
+
+///@} // end of implicit in-group elements [Doxygen]
+
+
+
+#define block33 template block<3,3>
+
+/**
+ * \name Articulated inertia - Coordinate transform
+ *
+ * These functions perform the coordinate transform of a spatial articulated
+ * inertia, in the special case of a mass-less handle (see chapter 7 of the RBDA
+ * book, §7.2.2, equation 7.23)
+ *
+ * Two specialized functions are available for the two cases of revolute and
+ * prismatic joint (which connects the subtree with the mass-less handle), since
+ * the inertia exhibits two different sparsity patterns.
+ *
+ * \param Ia_A the articulated inertia in A coordinates
+ * \param XM a spatial coordinate transform for motion vectors, in the form \c A_XM_B
+ * (that is, mapping forces from A to B coordinates)
+ * \param[out] Ia_B_const the same articulated inertia, but expressed in B
+ * coordinates. Note that the constness is casted away (trick required
+ * with Eigen)
+ */
+///@{
+/** \ingroup robcogen_commons */
+template <typename D1, typename D2, typename D3>
+void ctransform_Ia_revolute(
+ const MatrixBase<D1>& Ia_A,
+ const MatrixBase<D2>& XM,
+ const MatrixBase<D3>& Ia_B_const)
+{
+ typedef typename D1::Scalar Scalar; // we assume D1 D2 D3 use the same scalar type!
+
+ MatrixBase<D3>& Ia_B = const_cast<MatrixBase<D3>&>(Ia_B_const);
+ // Get the coefficients of the 3x3 rotation matrix B_E_A
+ // It is the transpose of the angular-angular block of the spatial transform
+ internal::Mat3x3Coefficients<Scalar> E(
+ XM(AX,AX),XM(AY,AX),XM(AZ,AX),
+ XM(AX,AY),XM(AY,AY),XM(AZ,AY),
+ XM(AX,AZ),XM(AY,AZ),XM(AZ,AZ));
+
+ // Recover the translation vector. See comment in method 'transformInertia'
+ Scalar rx = E.XY * XM(LZ,AX) + E.YY * XM(LZ,AY) + E.ZY * XM(LZ,AZ);
+ Scalar ry = E.XZ * XM(LX,AX) + E.YZ * XM(LX,AY) + E.ZZ * XM(LX,AZ);
+ Scalar rz = E.XX * XM(LY,AX) + E.YX * XM(LY,AY) + E.ZX * XM(LY,AZ);
+
+ // Angular-angular 3x3 sub-block:
+
+ internal::SymmMat3x3Coefficients<Scalar> aux1, aux2;
+
+ // ## compute I + (Crx + Crx^T) :
+
+ // Remember that, for a revolute joint, the structure of the matrix is as
+ // follows (note the zeros):
+ // [ia1, ia2, 0, ia4, ia5, ia6 ]
+ // [ia2, ia7, 0, ia8, ia9, ia10]
+ // [0 , 0, 0, 0, 0, 0 ]
+ // [ia4, ia8, 0, ia11, ia12, ia13]
+ // [ia5, ia9, 0, ia12, ia14, ia15]
+ // [ia6, ia10, 0, ia13, ia15, ia16]
+
+ // copying the coefficients results in slightly fewer invocations of the
+ // operator(int,int), in the rest of the function
+ internal::Mat3x3Coefficients<Scalar> C(
+ Ia_A(AX,LX),Ia_A(AX,LY),Ia_A(AX,LZ),
+ Ia_A(AY,LX),Ia_A(AY,LY),Ia_A(AY,LZ),
+ Scalar(0), Scalar(0), Scalar(0));
+
+ aux1.XX = Ia_A(AX,AX) + 2*C.XY*rz - 2*C.XZ*ry;
+ aux1.YY = Ia_A(AY,AY) + 2*C.YZ*rx - 2*C.YX*rz;
+ aux1.ZZ = Scalar(0);
+ aux1.XY = Ia_A(AX,AY) + C.YY*rz - C.XX*rz - C.YZ*ry + C.XZ*rx;
+ aux1.XZ = C.XX*ry - C.XY*rx;
+ aux1.YZ = C.YX*ry - C.YY*rx;
+
+ // ## compute (- rx M) (note the minus)
+ const internal::SymmMat3x3Coefficients<Scalar> M(
+ Ia_A(LX,LX),Ia_A(LX,LY),Ia_A(LX,LZ)
+ ,Ia_A(LY,LY),Ia_A(LY,LZ)
+ ,Ia_A(LZ,LZ));
+ internal::Mat3x3Coefficients<Scalar> rxM;
+ rxM.XX = rz*M.XY - ry*M.XZ;
+ rxM.YY = rx*M.YZ - rz*M.XY;
+ rxM.ZZ = ry*M.XZ - rx*M.YZ;
+ rxM.XY = rz*M.YY - ry*M.YZ;
+ rxM.YX = rx*M.XZ - rz*M.XX;
+ rxM.XZ = rz*M.YZ - ry*M.ZZ;
+ rxM.ZX = ry*M.XX - rx*M.XY;
+ rxM.YZ = rx*M.ZZ - rz*M.XZ;
+ rxM.ZY = ry*M.XY - rx*M.YY;
+
+ // ## compute (I + (Crx + Crx^T)) - (rxM) rx
+ aux1.XX += rxM.XY*rz - rxM.XZ*ry;
+ aux1.YY += rxM.YZ*rx - rxM.YX*rz;
+ aux1.ZZ += rxM.ZX*ry - rxM.ZY*rx;
+ aux1.XY += rxM.XZ*rx - rxM.XX*rz;
+ aux1.XZ += rxM.XX*ry - rxM.XY*rx;
+ aux1.YZ += rxM.YX*ry - rxM.YY*rx;
+
+ // ## compute E ( .. ) E^T
+ internal::rot_symmetric_EAET<Scalar>(E, aux1, aux2);
+
+ // Copy the result, angular-angular block of the output
+ Ia_B(AX,AX) = aux2.XX;
+ Ia_B(AY,AY) = aux2.YY;
+ Ia_B(AZ,AZ) = aux2.ZZ;
+ Ia_B(AY,AX) = Ia_B(AX,AY) = aux2.XY;
+ Ia_B(AZ,AX) = Ia_B(AX,AZ) = aux2.XZ;
+ Ia_B(AZ,AY) = Ia_B(AY,AZ) = aux2.YZ;
+ // aux2.write(Ia_B.block33(AX,AX)); // this is be equivalent, but it uses a block expression
+
+
+ // Angular-linear block (and linear-angular block)
+ // Calculate E ( C -rxM ) E^T
+ // - note that 'rxM' already contains the coefficients of (- rx * M)
+ // - for a revolute joint, the last line of C is zero
+ rxM.XX += C.XX;
+ rxM.XY += C.XY;
+ rxM.XZ += C.XZ;
+ rxM.YX += C.YX;
+ rxM.YY += C.YY;
+ rxM.YZ += C.YZ;
+
+ internal::Mat3x3Coefficients<Scalar> aux3;
+ internal::rot_EAET<Scalar>(E, rxM, aux3);
+ // copy the result, also to the symmetric 3x3 block
+ Ia_B(LX,AX) = Ia_B(AX,LX) = aux3.XX;
+ Ia_B(LY,AX) = Ia_B(AX,LY) = aux3.XY;
+ Ia_B(LZ,AX) = Ia_B(AX,LZ) = aux3.XZ;
+ Ia_B(LX,AY) = Ia_B(AY,LX) = aux3.YX;
+ Ia_B(LY,AY) = Ia_B(AY,LY) = aux3.YY;
+ Ia_B(LZ,AY) = Ia_B(AY,LZ) = aux3.YZ;
+ Ia_B(LX,AZ) = Ia_B(AZ,LX) = aux3.ZX;
+ Ia_B(LY,AZ) = Ia_B(AZ,LY) = aux3.ZY;
+ Ia_B(LZ,AZ) = Ia_B(AZ,LZ) = aux3.ZZ;
+
+ // Linear-linear block
+ internal::rot_symmetric_EAET<Scalar>(E, M, aux1);
+ Ia_B(LX,LX) = aux1.XX;
+ Ia_B(LY,LY) = aux1.YY;
+ Ia_B(LZ,LZ) = aux1.ZZ;
+ Ia_B(LY,LX) = Ia_B(LX,LY) = aux1.XY;
+ Ia_B(LZ,LX) = Ia_B(LX,LZ) = aux1.XZ;
+ Ia_B(LZ,LY) = Ia_B(LY,LZ) = aux1.YZ;
+ //aux1.write(Ia_B.block33(LX,LX)); // this is be equivalent, but it uses a block expression
+}
+
+/** \ingroup robcogen_commons */
+template <typename D1, typename D2, typename D3>
+void ctransform_Ia_prismatic(
+ const MatrixBase<D1>& Ia_A,
+ const MatrixBase<D2>& XM,
+ const MatrixBase<D3>& Ia_B_const)
+{
+ typedef typename D1::Scalar Scalar; // we assume D1 D2 D3 use the same scalar type!
+
+ MatrixBase<D3>& Ia_B = const_cast<MatrixBase<D3>&>(Ia_B_const);
+ // Get the coefficients of the 3x3 rotation matrix B_E_A
+ // It is the transpose of the angular-angular block of the spatial transform
+ internal::Mat3x3Coefficients<Scalar> E(
+ XM(AX,AX),XM(AY,AX),XM(AZ,AX),
+ XM(AX,AY),XM(AY,AY),XM(AZ,AY),
+ XM(AX,AZ),XM(AY,AZ),XM(AZ,AZ));
+
+ // Recover the translation vector. See comment in method 'transformInertia'
+ Scalar rx = E.XY * XM(LZ,AX) + E.YY * XM(LZ,AY) + E.ZY * XM(LZ,AZ);
+ Scalar ry = E.XZ * XM(LX,AX) + E.YZ * XM(LX,AY) + E.ZZ * XM(LX,AZ);
+ Scalar rz = E.XX * XM(LY,AX) + E.YX * XM(LY,AY) + E.ZX * XM(LY,AZ);
+
+ // Angular-angular 3x3 sub-block:
+
+ internal::SymmMat3x3Coefficients<Scalar> aux1, aux2;
+
+ // Remember that, for a prismatic joint, the structure of the matrix is as
+ // follows (note the zeros):
+
+ // [ia1, ia2, ia3, | ia4, ia5, 0 ]
+ // [ia2, ia6, ia7, | ia8, ia9, 0 ]
+ // [ia3, ia7, ia10,| ia11, ia12, 0 ]
+ // -----------------|-----------------
+ // [ia4, ia8, ia11,| ia13, ia14, 0 ]
+ // [ia5, ia9, ia12,| ia14, ia15, 0 ]
+ // [0 , 0, 0, | 0, 0, 0 ]
+
+ // [ I | C ]
+ // -----|-----
+ // [ C^T | M ]
+
+ // copying the coefficients results in slightly fewer invocations of the
+ // operator(int,int), in the rest of the function
+ internal::Mat3x3Coefficients<Scalar> C(
+ Ia_A(AX,LX), Ia_A(AX,LY), 0,
+ Ia_A(AY,LX), Ia_A(AY,LY), 0,
+ Ia_A(AZ,LX), Ia_A(AZ,LY), 0);
+
+ // ## compute I + (Crx + Crx^T) :
+ aux1.XX = Ia_A(AX,AX) + 2*C.XY*rz;
+ aux1.YY = Ia_A(AY,AY) - 2*C.YX*rz;
+ aux1.ZZ = Ia_A(AZ,AZ) + 2*C.ZX*ry - 2*C.ZY*rx;
+ aux1.XY = Ia_A(AX,AY) + (C.YY - C.XX)*rz;
+ aux1.XZ = Ia_A(AX,AZ) + C.ZY*rz + C.XX*ry - C.XY*rx;
+ aux1.YZ = Ia_A(AY,AZ) - C.ZX*rz + C.YX*ry - C.YY*rx;
+
+
+ const internal::SymmMat3x3Coefficients<Scalar> M(
+ Ia_A(LX,LX),Ia_A(LX,LY), 0
+ ,Ia_A(LY,LY), 0
+ , 0);
+ // ## compute the term (- rx M) (note the minus)
+ internal::Mat3x3Coefficients<Scalar> rxM;
+ rxM.XX = rz*M.XY;
+ rxM.XY = rz*M.YY;
+ rxM.YX = - rz*M.XX;
+ rxM.YY = - rz*M.XY;
+ rxM.ZX = ry*M.XX - rx*M.XY;
+ rxM.ZY = ry*M.XY - rx*M.YY;
+ rxM.XZ = 0.0;
+ rxM.YZ = 0.0;
+ rxM.ZZ = 0.0;
+
+ // ## compute (I + (Crx + Crx^T)) - (rxM) rx
+ aux1.XX += rxM.XY*rz;
+ aux1.YY += - rxM.YX*rz;
+ aux1.ZZ += rxM.ZX*ry - rxM.ZY*rx;
+ aux1.XY += - rxM.XX*rz;
+ aux1.XZ += rxM.XX*ry - rxM.XY*rx;
+ aux1.YZ += rxM.YX*ry - rxM.YY*rx;
+
+ // ## compute E ( I + Crx + (Crx)^T - rx M rx ) E^T
+ internal::rot_symmetric_EAET<Scalar>(E, aux1, aux2);
+
+ // Copy the result, angular-angular block of the output
+ Ia_B(AX,AX) = aux2.XX;
+ Ia_B(AY,AY) = aux2.YY;
+ Ia_B(AZ,AZ) = aux2.ZZ;
+ Ia_B(AY,AX) = Ia_B(AX,AY) = aux2.XY;
+ Ia_B(AZ,AX) = Ia_B(AX,AZ) = aux2.XZ;
+ Ia_B(AZ,AY) = Ia_B(AY,AZ) = aux2.YZ;
+ // aux2.write(Ia_B.block33(AX,AX)); // this is be equivalent, but it uses a block expression
+
+
+ // Angular-linear block (and linear-angular block)
+ // Calculate E ( C -rxM ) E^T
+ // - note that 'rxM' already contains the coefficients of (- rx * M)
+ // - for a prismatic joint, the last column of C is zero
+ rxM.XX += C.XX;
+ rxM.XY += C.XY;
+ //rxM.XZ stays zero
+ rxM.YX += C.YX;
+ rxM.YY += C.YY;
+ //rxM.YZ stays zero
+ rxM.ZX += C.ZX;
+ rxM.ZY += C.ZY;
+
+ internal::Mat3x3Coefficients<Scalar> aux3;
+ internal::rot_EAET<Scalar>(E, rxM, aux3); // TODO exploit the zeros in rxM
+ // copy the result, also to the symmetric 3x3 block
+ Ia_B(LX,AX) = Ia_B(AX,LX) = aux3.XX;
+ Ia_B(LY,AX) = Ia_B(AX,LY) = aux3.XY;
+ Ia_B(LZ,AX) = Ia_B(AX,LZ) = aux3.XZ;
+ Ia_B(LX,AY) = Ia_B(AY,LX) = aux3.YX;
+ Ia_B(LY,AY) = Ia_B(AY,LY) = aux3.YY;
+ Ia_B(LZ,AY) = Ia_B(AY,LZ) = aux3.YZ;
+ Ia_B(LX,AZ) = Ia_B(AZ,LX) = aux3.ZX;
+ Ia_B(LY,AZ) = Ia_B(AZ,LY) = aux3.ZY;
+ Ia_B(LZ,AZ) = Ia_B(AZ,LZ) = aux3.ZZ;
+
+ // Linear-linear block
+ internal::rot_symmetric_EAET<Scalar>(E, M, aux1); // TODO exploit the zeros in M
+ Ia_B(LX,LX) = aux1.XX;
+ Ia_B(LY,LY) = aux1.YY;
+ Ia_B(LZ,LZ) = aux1.ZZ;
+ Ia_B(LY,LX) = Ia_B(LX,LY) = aux1.XY;
+ Ia_B(LZ,LX) = Ia_B(LX,LZ) = aux1.XZ;
+ Ia_B(LZ,LY) = Ia_B(LY,LZ) = aux1.YZ;
+ //aux1.write(Ia_B.block33(LX,LX)); // this is be equivalent, but it uses a block expression
+}
+///@}
+#undef block33
+
+/**
+ * \name Articulated inertia - Computation
+ *
+ * These functions calculate the spatial articulated inertia of a subtree, in
+ * the special case of a mass-less handle (see chapter 7 of the RBDA
+ * book, §7.2.2, equation 7.23)
+ *
+ * Two specialized functions are available for the two cases of revolute and
+ * prismatic joint (which connects the subtree with the mass-less handle), since
+ * the inertia exhibits two different sparsity patterns.
+ *
+ * \param IA the regular articulated inertia of some subtree
+ * \param U the U term for the current joint (cfr. eq. 7.43 of RBDA)
+ * \param D the D term for the current joint (cfr. eq. 7.44 of RBDA)
+ * \param[out] Ia_const the articulated inertia for the same subtree, but
+ * propagated across the current joint. The matrix is assumed to be
+ * already initialized with zeros, at least in the row and column which
+ * is known to be zero. In other words, those elements are not computed
+ * nor assigned in this function.
+ * Note that the constness of the argument is
+ * casted away (trick required with Eigen), as this is an output
+ * argument.
+ */
+///@{
+/** \ingroup robcogen_commons */
+template <typename D1, typename D2>
+void compute_Ia_revolute(
+ const MatrixBase<D1>& IA,
+ const Vec6<typename D1::Scalar>& U,
+ const typename D1::Scalar& D,
+ const MatrixBase<D2>& Ia_const)
+{
+ typedef typename D1::Scalar Scalar; // we assume D1 D2 use the same scalar type!
+
+ MatrixBase<D2>& Ia = const_cast<MatrixBase<D2>&>(Ia_const);
+
+ Scalar UD_AX = U(AX) / D;
+ Scalar UD_AY = U(AY) / D;
+ //Scalar UD_AZ = U(AZ) / D;
+ Scalar UD_LX = U(LX) / D;
+ Scalar UD_LY = U(LY) / D;
+ Scalar UD_LZ = U(LZ) / D;
+
+ Ia(AX,AX) = IA(AX,AX) - U(AX)*UD_AX;
+ Ia(AX,AY) = Ia(AY,AX) = IA(AX,AY) - U(AX)*UD_AY;
+ //Ia(AX,AZ) = Ia(AZ,AX) = 0; // it is assumed to be set already
+ Ia(AX,LX) = Ia(LX,AX) = IA(AX,LX) - U(AX)*UD_LX;
+ Ia(AX,LY) = Ia(LY,AX) = IA(AX,LY) - U(AX)*UD_LY;
+ Ia(AX,LZ) = Ia(LZ,AX) = IA(AX,LZ) - U(AX)*UD_LZ;
+
+ Ia(AY,AY) = IA(AY,AY) - U(AY)*UD_AY;
+ //Ia(AY,AZ) = Ia(AZ,AY) = 0; // it is assumed to be set already
+ Ia(AY,LX) = Ia(LX,AY) = IA(AY,LX) - U(AY)*UD_LX;
+ Ia(AY,LY) = Ia(LY,AY) = IA(AY,LY) - U(AY)*UD_LY;
+ Ia(AY,LZ) = Ia(LZ,AY) = IA(AY,LZ) - U(AY)*UD_LZ;
+
+ //The whole row AZ it is assumed to be already set to zero
+
+ Ia(LX,LX) = IA(LX,LX) - U(LX)*UD_LX;
+ Ia(LX,LY) = Ia(LY,LX) = IA(LX,LY) - U(LX)*UD_LY;
+ Ia(LX,LZ) = Ia(LZ,LX) = IA(LX,LZ) - U(LX)*UD_LZ;
+
+ Ia(LY,LY) = IA(LY,LY) - U(LY)*UD_LY;
+ Ia(LY,LZ) = Ia(LZ,LY) = IA(LY,LZ) - U(LY)*UD_LZ;
+
+ Ia(LZ,LZ) = IA(LZ,LZ) - U(LZ)*UD_LZ;
+}
+
+/** \ingroup robcogen_commons */
+template <typename D1, typename D2>
+void compute_Ia_prismatic(
+ const MatrixBase<D1>& IA,
+ const Vec6<typename D1::Scalar>& U,
+ const typename D1::Scalar& D,
+ const MatrixBase<D2>& Ia_const)
+{
+ typedef typename D1::Scalar Scalar; // we assume D1 D2 use the same scalar type!
+
+ MatrixBase<D2>& Ia = const_cast<MatrixBase<D2>&>(Ia_const);
+
+ Scalar UD_AX = U(AX) / D;
+ Scalar UD_AY = U(AY) / D;
+ Scalar UD_AZ = U(AZ) / D;
+ Scalar UD_LX = U(LX) / D;
+ Scalar UD_LY = U(LY) / D;
+ //Scalar UD_LZ = U(LZ) / D;
+
+ Ia(AX,AX) = IA(AX,AX) - U(AX)*UD_AX;
+ Ia(AX,AY) = Ia(AY,AX) = IA(AX,AY) - U(AX)*UD_AY;
+ Ia(AX,AZ) = Ia(AZ,AX) = IA(AX,AZ) - U(AX)*UD_AZ;
+ Ia(AX,LX) = Ia(LX,AX) = IA(AX,LX) - U(AX)*UD_LX;
+ Ia(AX,LY) = Ia(LY,AX) = IA(AX,LY) - U(AX)*UD_LY;
+ //Ia(AX,LZ) = Ia(LZ,AX) = 0; // it is assumed to be set already
+
+ Ia(AY,AY) = IA(AY,AY) - U(AY)*UD_AY;
+ Ia(AY,AZ) = Ia(AZ,AY) = IA(AY,AZ) - U(AY)*UD_AZ;
+ Ia(AY,LX) = Ia(LX,AY) = IA(AY,LX) - U(AY)*UD_LX;
+ Ia(AY,LY) = Ia(LY,AY) = IA(AY,LY) - U(AY)*UD_LY;
+ //Ia(AY,LZ) = Ia(LZ,AY) = 0; // it is assumed to be set already
+
+ Ia(AZ,AZ) = IA(AZ,AZ) - U(AZ)*UD_AZ;
+ Ia(AZ,LX) = Ia(LX,AZ) = IA(AZ,LX) - U(AZ)*UD_LX;
+ Ia(AZ,LY) = Ia(LY,AZ) = IA(AZ,LY) - U(AZ)*UD_LY;
+ //Ia(AZ,LZ) = Ia(LZ,AZ) = 0; // it is assumed to be set already
+
+ Ia(LX,LX) = IA(LX,LX) - U(LX)*UD_LX;
+ Ia(LX,LY) = Ia(LY,LX) = IA(LX,LY) - U(LX)*UD_LY;
+ //Ia(LX,LZ) = Ia(LZ,LX) = 0; // it is assumed to be set already
+
+ Ia(LY,LY) = IA(LY,LY) - U(LY)*UD_LY;
+ //Ia(LY,LZ) = Ia(LZ,LY) = 0; // it is assumed to be set already
+
+ //Ia(LZ,LZ) = 0; // it is assumed to be set already
+}
+///@}
+
+
+
+
+}
+}
+
+#endif
diff --git a/ct_rbd/include/iit/rbd/traits/DoubleTrait.h b/ct_rbd/include/iit/rbd/traits/DoubleTrait.h
new file mode 100644
index 0000000..baf1d43
--- /dev/null
+++ b/ct_rbd/include/iit/rbd/traits/DoubleTrait.h
@@ -0,0 +1,34 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace iit {
+namespace rbd {
+
+struct DoubleTrait {
+
+ typedef double Scalar;
+
+ inline static Scalar sin(const Scalar& x) { return std::sin(x); }
+ inline static Scalar cos(const Scalar& x) { return std::cos(x); }
+ inline static Scalar tan(const Scalar& x) { return std::tan(x); }
+ inline static Scalar sinh(const Scalar& x) { return std::sinh(x); }
+ inline static Scalar cosh(const Scalar& x) { return std::cosh(x); }
+ inline static Scalar tanh(const Scalar& x) { return std::tanh(x); }
+ inline static Scalar exp(const Scalar& x) { return std::exp(x); }
+ inline static Scalar fabs(const Scalar& x) { return std::fabs(x); }
+
+ template <int Rows, int Cols>
+ inline static Eigen::Matrix<Scalar, Cols, 1> solve(const Eigen::Matrix<Scalar, Rows, Cols>& A, const Eigen::Matrix<Scalar, Rows, 1>& b)
+ {
+ return A.inverse()*b;
+ }
+
+};
+
+}
+}
diff --git a/ct_rbd/include/iit/rbd/traits/FloatTrait.h b/ct_rbd/include/iit/rbd/traits/FloatTrait.h
new file mode 100644
index 0000000..9113375
--- /dev/null
+++ b/ct_rbd/include/iit/rbd/traits/FloatTrait.h
@@ -0,0 +1,35 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+namespace iit {
+namespace rbd {
+
+
+
+struct FloatTrait {
+
+ typedef float Scalar;
+
+ inline static Scalar sin(const Scalar& x) { return std::sin(x); }
+ inline static Scalar cos(const Scalar& x) { return std::cos(x); }
+ inline static Scalar tan(const Scalar& x) { return std::tan(x); }
+ inline static Scalar sinh(const Scalar& x) { return std::sinh(x); }
+ inline static Scalar cosh(const Scalar& x) { return std::cosh(x); }
+ inline static Scalar tanh(const Scalar& x) { return std::tanh(x); }
+ inline static Scalar exp(const Scalar& x) { return std::exp(x); }
+ inline static Scalar fabs(const Scalar& x) { return std::fabs(x); }
+
+ template <int Rows, int Cols>
+ inline static Eigen::Matrix<Scalar, Cols, 1> solve(const Eigen::Matrix<Scalar, Rows, Cols>& A, const Eigen::Matrix<Scalar, Rows, 1>& b)
+ {
+ return A.inverse()*b;
+ }
+
+};
+
+}
+}
diff --git a/ct_rbd/include/iit/rbd/traits/TraitSelector.h b/ct_rbd/include/iit/rbd/traits/TraitSelector.h
new file mode 100644
index 0000000..95d59fa
--- /dev/null
+++ b/ct_rbd/include/iit/rbd/traits/TraitSelector.h
@@ -0,0 +1,37 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "FloatTrait.h"
+#include "DoubleTrait.h"
+
+namespace iit {
+namespace rbd {
+namespace tpl {
+
+template <typename SCALAR>
+struct TraitSelector
+{
+
+};
+
+template <>
+struct TraitSelector<double>
+{
+ typedef DoubleTrait Trait;
+};
+
+template <>
+struct TraitSelector<float>
+{
+ typedef FloatTrait Trait;
+};
+
+} //namespace tpl
+} // namespace rbd
+} // namespace iit
+
diff --git a/ct_rbd/include/iit/rbd/types.h b/ct_rbd/include/iit/rbd/types.h
new file mode 100644
index 0000000..0f9dd36
--- /dev/null
+++ b/ct_rbd/include/iit/rbd/types.h
@@ -0,0 +1,36 @@
+/* CPYHDR { */
+/*
+ * This file is part of the 'iit-rbd' library.
+ * Copyright © 2015 2016, Marco Frigerio (marco.frigerio@iit.it)
+ *
+ * See the LICENSE file for more information.
+ */
+/* } CPYHDR */
+#ifndef IIT_RBD_TYPES_H_
+#define IIT_RBD_TYPES_H_
+
+#include "rbd.h"
+#include "InertiaMatrix.h"
+
+
+namespace iit {
+namespace rbd {
+
+#define TPL template<typename S>
+
+TPL using Velocity = typename Core<S>::VelocityVector;
+TPL using Force = typename Core<S>::ForceVector;
+TPL using Mat33 = typename Core<S>::Matrix33;
+TPL using Mat66 = typename Core<S>::Matrix66;
+TPL using Vec3 = typename Core<S>::Vector3;
+TPL using Vec6 = typename Core<S>::Vector6;
+
+TPL using InertiaMat = tpl::InertiaMatrixDense<S>;
+
+#undef TPL
+
+}
+}
+
+
+#endif
diff --git a/ct_rbd/include/iit/rbd/utils.h b/ct_rbd/include/iit/rbd/utils.h
new file mode 100644
index 0000000..12ca355
--- /dev/null
+++ b/ct_rbd/include/iit/rbd/utils.h
@@ -0,0 +1,220 @@
+/* CPYHDR { */
+/*
+ * This file is part of the 'iit-rbd' library.
+ * Copyright © 2015 2016, Marco Frigerio (marco.frigerio@iit.it)
+ *
+ * See the LICENSE file for more information.
+ */
+/* } CPYHDR */
+#ifndef IIT_RBD_UTILS_H_
+#define IIT_RBD_UTILS_H_
+
+#include <cmath>
+#include <ctime>
+#include <iostream>
+
+#include "rbd.h"
+#include "types.h"
+
+namespace iit {
+namespace rbd {
+
+class Utils {
+ public:
+ struct InertiaMoments {
+ double ixx; double iyy; double izz; double ixy; double ixz; double iyz;
+ };
+
+ /**
+ * Coefficient-wise, unary operator that evaluates to zero for each element
+ * of the matrix expression whose absolute value is below a threshold.
+ * Useful for printing matrices, approximating with 0 small coefficients.
+ */
+ template <typename Scalar>
+ struct CwiseAlmostZeroOp {
+ CwiseAlmostZeroOp(const Scalar& threshold) : thresh(threshold) {}
+ const Scalar operator()(const Scalar& x) const {
+ return std::abs(x) < thresh ? 0 : x;
+ }
+ private:
+ Scalar thresh;
+ };
+public:
+ template <typename Scalar>
+ static Mat33<Scalar> buildInertiaTensor(
+ Scalar Ixx, Scalar Iyy, Scalar Izz,
+ Scalar Ixy, Scalar Ixz, Scalar Iyz)
+ {
+ Mat33<Scalar> I;
+ I << Ixx, -Ixy, -Ixz,
+ -Ixy, Iyy, -Iyz,
+ -Ixz, -Iyz, Izz;
+ return I;
+ }
+
+ static Matrix33d buildCrossProductMatrix(const Vector3d& in) {
+ Matrix33d out;
+ out << 0 , -in(2), in(1),
+ in(2), 0 , -in(0),
+ -in(1), in(0), 0;
+ return out;
+ }
+
+ // Put implementation here in header even if it is not inline, since
+ // this is a template function
+ /**
+ * Suppose B is rotated respect to A by rx, than ry, than rz.
+ * Fills the last parameter with the 3x3 rotation matrix A --> B ('B_X_A')
+ */
+ template <typename Derived>
+ static void fillAsRotationMatrix(double rx, double ry, double rz,
+ const MatrixBase<Derived>& mx)
+ {
+ eigen_assert(mx.rows() == 3 && mx.cols() == 3);
+
+ static double cx;
+ static double sx;
+ static double cy;
+ static double sy;
+ static double cz;
+ static double sz;
+ cx = std::cos(rx);
+ sx = std::sin(rx);
+ cy = std::cos(ry);
+ sy = std::sin(ry);
+ cz = std::cos(rz);
+ sz = std::sin(rz);
+
+ const_cast<MatrixBase<Derived>&>(mx)
+ << cy*cz, cx*sz+sx*sy*cz, sx*sz-cx*sy*cz,
+ -cy*sz, cx*cz-sx*sy*sz, cx*sy*sz+sx*cz,
+ sy, -sx*cy, cx*cy;
+ }
+
+ /**
+ * Suppose B is rotated respect to A by rx, than ry, than rz.
+ * \return the 3x3 rotation matrix A --> B ('B_X_A')
+ */
+ static Matrix33d buildRotationMatrix(double rx, double ry, double rz) {
+ double cx = std::cos(rx);
+ double sx = std::sin(rx);
+ double cy = std::cos(ry);
+ double sy = std::sin(ry);
+ double cz = std::cos(rz);
+ double sz = std::sin(rz);
+ Matrix33d ret;
+ ret << cy*cz, cx*sz+sx*sy*cz, sx*sz-cx*sy*cz,
+ -cy*sz, cx*cz-sx*sy*sz, cx*sy*sz+sx*cz,
+ sy, -sx*cy, cx*cy;
+ return ret;
+ }
+
+#define block31 template block<3,1>
+#define block33 template block<3,3>
+
+ template <typename Scalar>
+ static void fillAsMotionCrossProductMx(const Vec6<Scalar>& v, Mat66<Scalar>& mx) {
+ fillAsCrossProductMatrix(v.block31(0,0), mx.block33(0,0));
+ mx.block33(0,3).setZero();
+ fillAsCrossProductMatrix(v.block31(3,0), mx.block33(3,0));
+ mx.block33(3,3) = mx.block33(0,0);
+ }
+ template <typename Scalar>
+ static void fillAsForceCrossProductMx(const Vec6<Scalar>& v, Mat66<Scalar>& mx) {
+ fillAsCrossProductMatrix(v.block31(0,0), mx.block33(0,0));
+ fillAsCrossProductMatrix(v.block31(3,0), mx.block33(0,3));
+ mx.block33(3,0).setZero();
+ mx.block33(3,3) = mx.block33(0,0);
+ }
+
+ template <typename D1, typename D2>
+ static void fillAsCrossProductMatrix(const MatrixBase<D2>& in, const MatrixBase<D1>& mx)
+ {
+ eigen_assert(mx.rows() == 3 && mx.cols() == 3);
+ typedef typename D2::Scalar Scalar;
+
+ const_cast< MatrixBase<D1>& >(mx)
+ << static_cast<Scalar>(0.0) , -in(2), in(1),
+ in(2), static_cast<Scalar>(0.0) , -in(0),
+ -in(1), in(0), static_cast<Scalar>(0.0);
+ }
+
+ template <typename Derived>
+ static const MatrixBlock<const Derived,3,1> positionVector(const MatrixBase<Derived>& homT) {
+ eigen_assert(homT.rows() == 4 && homT.cols() == 4); // weak way to check if it is a homogeneous transform
+ return homT.block31(0,3);
+ }
+ template <typename Derived>
+ static const MatrixBlock<const Derived,3,3> rotationMx(const MatrixBase<Derived>& homT) {
+ eigen_assert(homT.rows() == 4 && homT.cols() == 4); // weak way to check if it is a homogeneous transform
+ return homT.block33(0,0);
+ }
+ template <typename Derived>
+ static const MatrixBlock<const Derived,3,1> zAxis(const MatrixBase<Derived>& homT) {
+ eigen_assert(homT.rows() == 4 && homT.cols() == 4); // weak way to check if it is a homogeneous transform
+ return homT.block31(0,2);
+ }
+
+#undef block31
+#undef block33
+
+ /**
+ * Applies a roto-translation to the given 3D vector.
+ * This function is provided for convenience, since a direct matrix product
+ * between a 4x4 matrix and a 3x1 vector is obviously not possible. It should
+ * also be slightly more efficient than taking the homogeneous representation
+ * of the 3D vector (4x1) and then compute the matrix product.
+ *
+ * \param homT a 4x4 homogeneous coordinate transform encoding the rotation
+ * and the translation
+ * \param vect3d the 3x1 vector to be transformed
+ * \return the 3D vector that results from the rotation plus translation
+ */
+ template <typename Derived, typename Other>
+ static Vector3d transform(
+ const MatrixBase<Derived>& homT,
+ const MatrixBase<Other>& vect3d)
+ {
+ eigen_assert(homT.rows() == 4 && homT.cols() == 4); // weak way to check if it is a homogeneous transform
+ eigen_assert(vect3d.rows() == 3 && vect3d.cols() == 1); // check if it is a 3d column
+ return rotationMx(homT) * vect3d + positionVector(homT);
+ }
+
+ static void randomVec(Vector6D& vec)
+ {
+ std::srand(std::time(NULL));
+ for(int i=0; i<6; i++) {
+ vec(i) = ((double)std::rand()) / RAND_MAX;
+ }
+ }
+
+ static void randomGravity(VelocityVector& g)
+ {
+ angularPart(g).setZero();
+ std::srand(std::time(NULL));
+ g(LX) = ((double)std::rand()) / RAND_MAX;
+ g(LY) = ((double)std::rand()) / RAND_MAX;
+ g(LZ) = ((double)std::rand()) / RAND_MAX;
+ linearPart(g) = (linearPart(g) / linearPart(g).norm()) * iit::rbd::g;
+ }
+
+ template <class RT>
+ static void cmdlineargs_spatialv(int argc, char** argv,
+ iit::rbd::Vector6D& vec)
+ {
+ if(argc < 6) {
+ std::cerr << "Not enough arguments" << std::endl;
+ exit(-1);
+ }
+ for(int i=0; i<6; i++) {
+ vec(i) = std::atof(argv[i]);
+ }
+ }
+
+};
+
+
+}
+}
+
+#endif /* IIT_RBD_UTILS_H_ */
diff --git a/ct_rbd/package.xml b/ct_rbd/package.xml
new file mode 100644
index 0000000..aa426b9
--- /dev/null
+++ b/ct_rbd/package.xml
@@ -0,0 +1,26 @@
+<package>
+ <name>ct_rbd</name>
+ <version>0.2.1</version>
+ <description>
+ Rigid Body Dynamics module of ADRL control toolbox
+ </description>
+ <maintainer email="neunertm@gmail.com">Michael Neunert</maintainer>
+ <maintainer email="mgiftthaler@ethz.ch">Markus Giftthaler</maintainer>
+ <license>Apache v2</license>
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <build_depend>roscpp</build_depend>
+ <build_depend>ct_core</build_depend>
+ <build_depend>ct_optcon</build_depend>
+ <build_depend>eigen</build_depend>
+ <build_depend>cmake_modules</build_depend>
+ <build_depend>kindr</build_depend>
+
+ <run_depend>roscpp</run_depend>
+ <run_depend>ct_core</run_depend>
+ <run_depend>ct_optcon</run_depend>
+ <run_depend>cmake_modules</run_depend>
+ <run_depend>kindr</run_depend>
+
+</package>
diff --git a/ct_rbd/src/kindrEulerInverseRotateTest.cpp b/ct_rbd/src/kindrEulerInverseRotateTest.cpp
new file mode 100644
index 0000000..1ae971c
--- /dev/null
+++ b/ct_rbd/src/kindrEulerInverseRotateTest.cpp
@@ -0,0 +1,71 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#include <chrono>
+#include <kindr/Core>
+
+int main()
+{
+ static const size_t nTests = 1000000;
+ std::vector<kindr::EulerAnglesXyzD, Eigen::aligned_allocator<kindr::EulerAnglesXyzD>> angles(nTests);
+
+ typedef std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> test_vector_t;
+ test_vector_t vectors(nTests);
+ test_vector_t vectorsRotatedNormal(nTests);
+ test_vector_t vectorsRotatedMatrix(nTests);
+
+ std::cout << "running " << nTests << " rotation tests" << std::endl;
+
+ for (size_t i = 0; i < nTests; i++)
+ {
+ angles[i].setRandom();
+ vectors[i].setRandom();
+ }
+
+ std::cout << "timing angle.inverseRotate(vector)" << std::endl;
+ auto start = std::chrono::high_resolution_clock::now();
+ for (size_t i = 0; i < nTests; i++)
+ {
+ vectorsRotatedNormal[i] = angles[i].inverseRotate(vectors[i]);
+ }
+ auto end = std::chrono::high_resolution_clock::now();
+ auto diff = end - start;
+ size_t msTotal = std::chrono::duration<double, std::milli>(diff).count();
+ std::cout << "Total time: " << msTotal << " ms. Average: " << msTotal / double(nTests) << " ms" << std::endl;
+
+ std::cout << "timing angle.toRotationMatrix().inverted().rotate(vector)" << std::endl;
+ start = std::chrono::high_resolution_clock::now();
+ for (size_t i = 0; i < nTests; i++)
+ {
+ vectorsRotatedMatrix[i] = kindr::RotationMatrixD(angles[i]).inverted().rotate(vectors[i]);
+ }
+ end = std::chrono::high_resolution_clock::now();
+ diff = end - start;
+ msTotal = std::chrono::duration<double, std::milli>(diff).count();
+ std::cout << "Total time: " << msTotal << " ms. Average: " << msTotal / double(nTests) << " ms" << std::endl;
+
+ double error = 0.0;
+ double maxError = 0.0;
+ size_t maxErrorIndex = 0;
+ for (size_t i = 0; i < nTests; i++)
+ {
+ double err = (vectorsRotatedNormal[i] - vectorsRotatedMatrix[i]).norm();
+ if (err > maxError)
+ {
+ maxError = err;
+ maxErrorIndex = i;
+ }
+ error += err;
+ }
+ std::cout << "average error: " << error / double(nTests) << std::endl;
+ std::cout << "max error " << maxError << " for pair: " << std::endl;
+ std::cout << "euler angles: " << angles[maxErrorIndex].toImplementation().transpose() << std::endl;
+ std::cout << "vector to rotate: " << vectors[maxErrorIndex].transpose() << std::endl;
+ std::cout << "vector inverse rotated (direct): " << vectorsRotatedNormal[maxErrorIndex].transpose() << std::endl;
+ std::cout << "vector inverse rotated (rotation matrix transpose): "
+ << vectorsRotatedMatrix[maxErrorIndex].transpose() << std::endl;
+}
diff --git a/ct_rbd/test/models/testIrb4600/RobCoGenTestIrb4600.h b/ct_rbd/test/models/testIrb4600/RobCoGenTestIrb4600.h
new file mode 100644
index 0000000..981d40c
--- /dev/null
+++ b/ct_rbd/test/models/testIrb4600/RobCoGenTestIrb4600.h
@@ -0,0 +1,45 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <Eigen/Core>
+
+#include <ct/rbd/rbd.h>
+
+#include "generated/declarations.h"
+#include "generated/forward_dynamics.h"
+#include "generated/inertia_properties.h"
+#include "generated/inverse_dynamics.h"
+#include "generated/jacobians.h"
+#include "generated/jsim.h"
+#include "generated/transforms.h"
+#include "generated/link_data_map.h"
+#include "generated/traits.h"
+
+
+#define ROBCOGEN_NS testirb4600 // defines the NS of the robot in robcogen, e.g. iit::<ROBCOGEN_NS>
+#define TARGET_NS \
+ TestIrb4600 // defines the NS where all robot definitions go. Here ct::rbd::TestIrb4600. This defines ct::rbd::TestIrb4600::Dynamics etc.
+
+
+// define the links
+#define CT_BASE fr_link0
+#define CT_L0 fr_link1
+#define CT_L1 fr_link2
+#define CT_L2 fr_link3
+#define CT_L3 fr_link4
+#define CT_L4 fr_link5
+#define CT_L5 fr_link6
+
+// define single end effector (could also be multiple)
+#define CT_N_EE 1;
+#define CT_EE0 fr_ee
+#define CT_EE0_IS_ON_LINK 5
+#define CT_EE0_FIRST_JOINT 0
+#define CT_EE0_LAST_JOINT 5
+
+#include <ct/rbd/robot/robcogen/robcogenHelpers.h>
diff --git a/ct_rbd/test/models/testIrb4600/description/testirb4600.dtdsl b/ct_rbd/test/models/testIrb4600/description/testirb4600.dtdsl
new file mode 100644
index 0000000..c30b1b7
--- /dev/null
+++ b/ct_rbd/test/models/testIrb4600/description/testirb4600.dtdsl
@@ -0,0 +1,40 @@
+Robot testirb4600
+
+Frames {
+ fr_link0, fr_link1, fr_link2, fr_link3, fr_link4, fr_link5, fr_link6, fr_ee, com0, com1, com2, com3, com4,com5,com6
+}
+
+Transforms {
+ base=fr_link0, target=fr_link1
+ base=fr_link0, target=fr_link2
+ base=fr_link0, target=fr_link3
+ base=fr_link0, target=fr_link4
+ base=fr_link0, target=fr_link5
+ base=fr_link0, target=fr_link6
+ base=fr_link0, target=fr_ee
+ base=fr_link0, target=com0
+ base=fr_link0, target=com1
+ base=fr_link0, target=com2
+ base=fr_link0, target=com3
+ base=fr_link0, target=com4
+ base=fr_link0, target=com5
+ base=fr_link0, target=com6
+ base=fr_link1, target=fr_link0
+ base=fr_link2, target=fr_link0
+ base=fr_link3, target=fr_link0
+ base=fr_link4, target=fr_link0
+ base=fr_link5, target=fr_link0
+ base=fr_link6, target=fr_link0
+ base=fr_ee , target=fr_link0
+ base=com0 , target=fr_link0
+ base=com1 , target=fr_link0
+ base=com2 , target=fr_link0
+ base=com3 , target=fr_link0
+ base=com4 , target=fr_link0
+ base=com5 , target=fr_link0
+ base=com6 , target=fr_link0
+}
+
+Jacobians {
+ base=fr_link0, target=fr_ee
+}
diff --git a/ct_rbd/test/models/testIrb4600/description/testirb4600.kindsl b/ct_rbd/test/models/testIrb4600/description/testirb4600.kindsl
new file mode 100644
index 0000000..fa9c64f
--- /dev/null
+++ b/ct_rbd/test/models/testIrb4600/description/testirb4600.kindsl
@@ -0,0 +1,184 @@
+Robot testirb4600 {
+
+RobotBase link0 {
+ inertia_params {
+ mass = 0.0
+ CoM = (0.0, 0.0, 0.075)
+ Iy=0.0 Ix=0.0 Iz=0.0 Ixy=0.0 Ixz=0.0 Iyz=0.0
+ }
+ children {
+ link1 via jA
+ }
+ frames {
+ com0 {
+ translation = (0.0, 0.0, 0.075)
+ rotation = (0.0, 0.0, 0.0)
+ }
+ }
+}
+
+link link1 {
+ id = 1
+ inertia_params {
+ mass = 120.0
+ CoM = (0.0, 0.0, 0.0)
+ Ix=2.52 Iy=2.52 Iz=1.8 Ixy=0.0 Ixz=0.0 Iyz=0.0
+ ref_frame = com1
+ }
+
+ children {
+ link2 via jB
+ }
+frames {
+ com1 {
+ translation = (0.09, 0.0, 0.25)
+ rotation = (0.0, 0.0, 0.0)
+ }
+ }
+}
+
+
+link link2 {
+ id = 2
+ inertia_params {
+ mass = 120.0
+ CoM = (0.0, 0.00, 0.0)
+ Ix=0.7 Iy=11.25 Iz=11.75 Ixy=0.0 Ixz=0.0 Iyz=0.0
+ ref_frame = com2
+ }
+
+ children {
+ link3 via jC
+ }
+ frames {
+ com2 {
+ translation = (0.47, 0.04, -0.17)
+ rotation = (0.0, 0.0, 0.0)
+ }
+ }
+}
+
+
+link link3 {
+ id = 3
+ inertia_params {
+ mass = 120.0
+ CoM = (0.0, 0.0, 0.0)
+ Ix=3.7 Iy=5.03 Iz=5.03 Ixy=0.0 Ixz=0.0 Iyz=0.0
+ ref_frame = com3
+ }
+
+ children {
+ link4 via jD
+ }
+ frames {
+ com3 {
+ translation = (0.035, -0.133, 0.044)
+ rotation = (0.0, 0.0, 0.0)
+ }
+ }
+}
+
+link link4 {
+ id = 4
+ inertia_params {
+ mass = 40.0
+ CoM = (0.0, 0.0, 0.0)
+ Ix=2.0 Iy=2.0 Iz=0.482 Ixy=0.0 Ixz=0.0 Iyz=0.0
+ ref_frame = com4
+ }
+
+ children {
+ link5 via jE
+ }
+ frames {
+ com4 {
+ translation = (0.0, 0.0, 0.5)
+ rotation = (0.0, 0.0, 0.0)
+ }
+ }
+}
+
+link link5 {
+ id = 5
+ inertia_params {
+ mass = 10.0
+ CoM = (0.0, 0.0, 0.0)
+ Ix=0.04 Iy=0.04 Iz=0.045 Ixy=0.0 Ixz=0.0 Iyz=0.0
+ ref_frame = com5
+ }
+
+ children {
+ link6 via jF
+ }
+ frames {
+ com5 {
+ translation = (0.07, 0.0, 0.0)
+ rotation = (0.0, 0.0, 0.0)
+ }
+ }
+}
+
+link link6 {
+ id = 6
+ inertia_params {
+ mass = 5.0
+ CoM = (0.081, -0.091, 0.232)
+ Ix=0.014 Iy=0.014 Iz=0.028 Ixy=0.0 Ixz=0.0 Iyz=0.0
+ }
+
+ children {}
+ frames {
+ fr_ee {
+ translation = (0.0, 0.0, 0.2)
+ rotation = (0.0, 0.0, 0.0) //roll - pitch - yaw
+ }
+ com6 {
+ translation = (0.0, 0.0, 0.1)
+ rotation = (0.0, 0.0, 0.0) //roll - pitch - yaw
+ }
+ }
+}
+
+r_joint jA {
+ ref_frame {
+ translation = (0.0, 0.0, 0.0)
+ rotation = (0.0, 0.0, 0.0)
+ }
+}
+
+r_joint jB {
+ ref_frame {
+ translation = (0.175, 0.0, 0.495)
+ rotation = (-PI/2.0, 0.0, -PI/2.0)
+ }
+}
+
+r_joint jC {
+ ref_frame {
+ translation = (1.095, 0.0, 0.0)
+ rotation = (0.0, 0.0, PI/2.0)
+ }
+}
+r_joint jD {
+ ref_frame {
+ translation = (0.332, -0.175, 0.0)
+ rotation = (0.0, PI/2.0, 0.0)
+ }
+}
+
+r_joint jE {
+ ref_frame {
+ translation = (0.0, 0.0, 0.939)
+ rotation = (0.0, -PI/2.0, 0.0)
+ }
+}
+
+r_joint jF {
+ ref_frame {
+ translation = (0.243, 0.0, 0.0)
+ rotation = (PI/2.0, PI/2.0, 0.0)
+ }
+}
+
+}
diff --git a/ct_rbd/test/models/testIrb4600/description/testirb4600.urdf b/ct_rbd/test/models/testIrb4600/description/testirb4600.urdf
new file mode 100644
index 0000000..8b2247d
--- /dev/null
+++ b/ct_rbd/test/models/testIrb4600/description/testirb4600.urdf
@@ -0,0 +1,143 @@
+<robot name="testirb4600">
+ <link name="link0">
+ </link>
+ <link name="link1">
+ <inertial>
+ <origin xyz="0.18000 0.00000 0.50000"/>
+ <mass value="120.00000"/>
+ <inertia ixx="-4.98000" iyy="-5.95200" izz="0.82800" ixy="-0.00000" ixz="2.70000" iyz="-0.00000"/>
+ </inertial>
+ <visual>
+ <geometry>
+ <cylinder length="0.6" radius="0.2"/>
+ </geometry>
+ </visual>
+ <collision>
+ <geometry>
+ <cylinder length="0.6" radius="0.2"/>
+ </geometry>
+ </collision>
+ </link>
+ <link name="link2">
+ <inertial>
+ <origin xyz="0.94000 0.08000 -0.34000"/>
+ <mass value="120.00000"/>
+ <inertia ixx="-2.96000" iyy="-18.72601" izz="-14.95000" ixy="2.25600" ixz="-9.58800" iyz="-0.81600"/>
+ </inertial><visual>
+ <geometry>
+ <cylinder length="0.6" radius="0.2"/>
+ </geometry>
+ </visual>
+ <collision>
+ <geometry>
+ <cylinder length="0.6" radius="0.2"/>
+ </geometry>
+ </collision>
+ </link>
+ <link name="link3">
+ <inertial>
+ <origin xyz="0.07000 -0.26600 0.08800"/>
+ <mass value="120.00000"/>
+ <inertia ixx="1.34500" iyy="4.65068" izz="2.76032" ixy="-0.55860" ixz="0.18480" iyz="-0.70224"/>
+ </inertial><visual>
+ <geometry>
+ <cylinder length="0.6" radius="0.2"/>
+ </geometry>
+ </visual>
+ <collision>
+ <geometry>
+ <cylinder length="0.6" radius="0.2"/>
+ </geometry>
+ </collision>
+ </link>
+ <link name="link4">
+ <inertial>
+ <origin xyz="0.00000 0.00000 1.00000"/>
+ <mass value="40.00000"/>
+ <inertia ixx="-8.00000" iyy="-8.00000" izz="0.48200" ixy="-0.00000" ixz="-0.00000" iyz="-0.00000"/>
+ </inertial><visual>
+ <geometry>
+ <cylinder length="0.6" radius="0.2"/>
+ </geometry>
+ </visual>
+ <collision>
+ <geometry>
+ <cylinder length="0.6" radius="0.2"/>
+ </geometry>
+ </collision>
+ </link>
+ <link name="link5">
+ <inertial>
+ <origin xyz="0.14000 0.00000 0.00000"/>
+ <mass value="10.00000"/>
+ <inertia ixx="0.04000" iyy="-0.00900" izz="-0.00400" ixy="-0.00000" ixz="-0.00000" iyz="-0.00000"/>
+ </inertial><visual>
+ <geometry>
+ <cylinder length="0.6" radius="0.2"/>
+ </geometry>
+ </visual>
+ <collision>
+ <geometry>
+ <cylinder length="0.6" radius="0.2"/>
+ </geometry>
+ </collision>
+ </link>
+ <link name="link6">
+ <inertial>
+ <origin xyz="0.08100 -0.09100 0.33200"/>
+ <mass value="5.00000"/>
+ <inertia ixx="-0.29652" iyy="-0.28792" izz="-0.04621" ixy="-0.03686" ixz="0.09396" iyz="-0.10556"/>
+ </inertial><visual>
+ <geometry>
+ <cylinder length="0.6" radius="0.2"/>
+ </geometry>
+ </visual>
+ <collision>
+ <geometry>
+ <cylinder length="0.6" radius="0.2"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="jA" type="revolute">
+ <origin xyz="0.00000 0.00000 0.00000" rpy="0.0 0.0 0.0"/>
+ <parent link="link0"/>
+ <child link="link1"/>
+ <limit effort="30" velocity="1.0"/>
+ <axis xyz="0 0 1"/>
+ </joint>
+ <joint name="jB" type="revolute">
+ <origin xyz="0.17500 0.00000 0.49500" rpy="2.3561944901923457 -1.5707962635746238 2.3561944901923457"/>
+ <parent link="link1"/>
+ <child link="link2"/>
+ <limit effort="30" velocity="1.0"/>
+ <axis xyz="0 0 1"/>
+ </joint>
+ <joint name="jC" type="revolute">
+ <origin xyz="1.09500 0.00000 0.00000" rpy="0.0 -0.0 1.5707963705062866"/>
+ <parent link="link2"/>
+ <child link="link3"/>
+ <limit effort="30" velocity="1.0"/>
+ <axis xyz="0 0 1"/>
+ </joint>
+ <joint name="jD" type="revolute">
+ <origin xyz="0.33200 -0.17500 0.00000" rpy="3.141592653589793 1.570796282091413 3.141592653589793"/>
+ <parent link="link3"/>
+ <child link="link4"/>
+ <limit effort="30" velocity="1.0"/>
+ <axis xyz="0 0 1"/>
+ </joint>
+ <joint name="jE" type="revolute">
+ <origin xyz="0.00000 0.00000 0.93900" rpy="3.141592653589793 -1.570796282091413 3.141592653589793"/>
+ <parent link="link4"/>
+ <child link="link5"/>
+ <limit effort="30" velocity="1.0"/>
+ <axis xyz="0 0 1"/>
+ </joint>
+ <joint name="jF" type="revolute">
+ <origin xyz="0.24300 0.00000 0.00000" rpy="1.5707963267948948 -4.371139000186238E-8 1.5707963705062866"/>
+ <parent link="link5"/>
+ <child link="link6"/>
+ <limit effort="30" velocity="1.0"/>
+ <axis xyz="0 0 1"/>
+ </joint>
+</robot>
diff --git a/ct_rbd/test/models/testIrb4600/generated/declarations.h b/ct_rbd/test/models/testIrb4600/generated/declarations.h
new file mode 100644
index 0000000..732f650
--- /dev/null
+++ b/ct_rbd/test/models/testIrb4600/generated/declarations.h
@@ -0,0 +1,52 @@
+#ifndef IIT_ROBOT_TESTIRB4600_DECLARATIONS_H_
+#define IIT_ROBOT_TESTIRB4600_DECLARATIONS_H_
+
+#include <Eigen/Dense>
+
+namespace iit {
+namespace testirb4600 {
+
+static const int JointSpaceDimension = 6;
+static const int jointsCount = 6;
+/** The total number of rigid bodies of this robot, including the base */
+static const int linksCount = 7;
+
+namespace tpl {
+template <typename SCALAR>
+using Column6d = Eigen::Matrix<SCALAR, 6, 1>;
+
+template <typename SCALAR>
+using JointState = Column6d<SCALAR>;
+}
+
+using Column6d = tpl::Column6d<double>;
+typedef Column6d JointState;
+
+enum JointIdentifiers {
+ JA = 0
+ , JB
+ , JC
+ , JD
+ , JE
+ , JF
+};
+
+enum LinkIdentifiers {
+ LINK0 = 0
+ , LINK1
+ , LINK2
+ , LINK3
+ , LINK4
+ , LINK5
+ , LINK6
+};
+
+static const JointIdentifiers orderedJointIDs[jointsCount] =
+ {JA,JB,JC,JD,JE,JF};
+
+static const LinkIdentifiers orderedLinkIDs[linksCount] =
+ {LINK0,LINK1,LINK2,LINK3,LINK4,LINK5,LINK6};
+
+}
+}
+#endif
diff --git a/ct_rbd/test/models/testIrb4600/generated/default_dynparams_getter.h b/ct_rbd/test/models/testIrb4600/generated/default_dynparams_getter.h
new file mode 100644
index 0000000..f1dfd4e
--- /dev/null
+++ b/ct_rbd/test/models/testIrb4600/generated/default_dynparams_getter.h
@@ -0,0 +1,30 @@
+#ifndef _TESTIRB4600_DEFAULT_GETTER_INERTIA_PARAMETERS_
+#define _TESTIRB4600_DEFAULT_GETTER_INERTIA_PARAMETERS_
+
+#include "dynamics_parameters.h"
+
+namespace iit {
+namespace testirb4600 {
+namespace dyn {
+
+class DefaultParamsGetter : public RuntimeParamsGetter
+{
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ DefaultParamsGetter() {
+ resetDefaults();
+ }
+ ~DefaultParamsGetter() {};
+
+ public:
+ void resetDefaults() {
+ }
+
+ private:
+ RuntimeInertiaParams values;
+};
+
+}
+}
+}
+#endif
diff --git a/ct_rbd/test/models/testIrb4600/generated/dynamics_parameters.h b/ct_rbd/test/models/testIrb4600/generated/dynamics_parameters.h
new file mode 100644
index 0000000..225b768
--- /dev/null
+++ b/ct_rbd/test/models/testIrb4600/generated/dynamics_parameters.h
@@ -0,0 +1,43 @@
+#ifndef _TESTIRB4600_RUNTIME_INERTIA_PARAMETERS_
+#define _TESTIRB4600_RUNTIME_INERTIA_PARAMETERS_
+
+namespace iit {
+namespace testirb4600 {
+namespace dyn {
+/**
+ * \defgroup dynparams Dynamics-parameters
+ * Facilities related to the parameters of the inertia properties of the
+ * robot testirb4600.
+ *
+ * Inertia parameters are non-constants used in the robot model, where the
+ * inertia properties (mass, center of mass, intertia tensor) of the links
+ * are specified. Since the value of such parameters must be resolved
+ * at runtime, we sometimes refer to them as "runtime parameters", "runtime
+ * dynamics parameters", "runtime inertia parameters", etc.
+ *
+ * Do not confuse them with the "inertia properties" of links, which
+ * unfortunately, in the literature, are commonly referred to as
+ * "inertia parameters"... Here, the parameters are the non-constant
+ * fields of the inertia properties.
+ */
+
+ /**
+ * A container for the set of non-constant inertia parameters of the robot testirb4600
+ * \ingroup dynparams
+ */
+ struct RuntimeInertiaParams {
+ };
+
+ /**
+ * The interface for classes that can compute the actual value of the
+ * non-constant inertia parameters of the robot testirb4600.
+ * \ingroup dynparams
+ */
+ class RuntimeParamsGetter {
+ public:
+ };
+
+}
+}
+}
+#endif
diff --git a/ct_rbd/test/models/testIrb4600/generated/forward_dynamics.h b/ct_rbd/test/models/testIrb4600/generated/forward_dynamics.h
new file mode 100644
index 0000000..21a4125
--- /dev/null
+++ b/ct_rbd/test/models/testIrb4600/generated/forward_dynamics.h
@@ -0,0 +1,189 @@
+#ifndef IIT_ROBOT_TESTIRB4600_FORWARD_DYNAMICS_H_
+#define IIT_ROBOT_TESTIRB4600_FORWARD_DYNAMICS_H_
+
+#include <Eigen/Dense>
+#include <iit/rbd/rbd.h>
+#include <iit/rbd/InertiaMatrix.h>
+#include <iit/rbd/utils.h>
+#include <iit/rbd/robcogen_commons.h>
+
+#include "declarations.h"
+#include "transforms.h"
+#include "inertia_properties.h"
+#include "link_data_map.h"
+
+namespace iit {
+namespace testirb4600 {
+namespace dyn {
+
+/**
+ * The Forward Dynamics routine for the robot testirb4600.
+ *
+ * The parameters common to most of the methods are the joint status \c q, the
+ * joint velocities \c qd and the joint forces \c tau. The accelerations \c qdd
+ * will be filled with the computed values. Overloaded methods without the \c q
+ * parameter use the current configuration of the robot; they are provided for
+ * the sake of efficiency, in case the kinematics transforms of the robot have
+ * already been updated elsewhere with the most recent configuration (eg by a
+ * call to setJointStatus()), so that it would be useless to compute them again.
+ */
+
+namespace tpl{
+
+template <typename TRAIT>
+class ForwardDynamics {
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ // Convenient type aliases:
+
+ typedef typename TRAIT::Scalar SCALAR;
+
+ typedef iit::rbd::Core<SCALAR> CoreS;
+
+ typedef LinkDataMap<typename CoreS::ForceVector> ExtForces;
+ typedef typename CoreS::ForceVector Force;
+ typedef typename CoreS::VelocityVector Velocity;
+ typedef typename CoreS::VelocityVector Acceleration;
+ typedef typename CoreS::Column6D Column6DS;
+ typedef typename iit::testirb4600::tpl::JointState<SCALAR> JointState;
+ typedef typename CoreS::Matrix66 Matrix66S;
+
+ typedef iit::rbd::tpl::InertiaMatrixDense<SCALAR> InertiaMatrix;
+ typedef iit::testirb4600::tpl::MotionTransforms<TRAIT> MTransforms;
+
+public:
+ /**
+ * Default constructor
+ * \param in the inertia properties of the links
+ * \param tr the container of all the spatial motion transforms of
+ * the robot testirb4600, which will be used by this instance
+ * to compute the dynamics.
+ */
+ ForwardDynamics(iit::testirb4600::dyn::tpl::InertiaProperties<TRAIT>& in, MTransforms& tr);
+ /** \name Forward dynamics
+ * The Articulated-Body-Algorithm to compute the joint accelerations
+ */ ///@{
+ /**
+ * \param qdd the joint accelerations vector (output parameter).
+ * \param q the joint status vector
+ * \param qd the joint velocities vector
+ * \param tau the joint forces (torque or force)
+ * \param fext the external forces, optional. Each force must be
+ * expressed in the reference frame of the link it is
+ * exerted on.
+ */
+ void fd(
+ JointState& qdd, // output parameter
+ const JointState& q, const JointState& qd, const JointState& tau, const ExtForces& fext = zeroExtForces);
+ void fd(
+ JointState& qdd, // output parameter
+ const JointState& qd, const JointState& tau, const ExtForces& fext = zeroExtForces);
+ ///@}
+
+ /** Updates all the kinematics transforms used by this instance. */
+ void setJointStatus(const JointState& q) const;
+
+private:
+ iit::testirb4600::dyn::tpl::InertiaProperties<TRAIT>* inertiaProps;
+ MTransforms* motionTransforms;
+
+ Matrix66S vcross; // support variable
+ Matrix66S Ia_r; // support variable, articulated inertia in the case of a revolute joint
+
+ // Link 'link1' :
+ Matrix66S link1_AI;
+ Velocity link1_a;
+ Velocity link1_v;
+ Velocity link1_c;
+ Force link1_p;
+
+ Column6DS link1_U;
+ SCALAR link1_D;
+ SCALAR link1_u;
+ // Link 'link2' :
+ Matrix66S link2_AI;
+ Velocity link2_a;
+ Velocity link2_v;
+ Velocity link2_c;
+ Force link2_p;
+
+ Column6DS link2_U;
+ SCALAR link2_D;
+ SCALAR link2_u;
+ // Link 'link3' :
+ Matrix66S link3_AI;
+ Velocity link3_a;
+ Velocity link3_v;
+ Velocity link3_c;
+ Force link3_p;
+
+ Column6DS link3_U;
+ SCALAR link3_D;
+ SCALAR link3_u;
+ // Link 'link4' :
+ Matrix66S link4_AI;
+ Velocity link4_a;
+ Velocity link4_v;
+ Velocity link4_c;
+ Force link4_p;
+
+ Column6DS link4_U;
+ SCALAR link4_D;
+ SCALAR link4_u;
+ // Link 'link5' :
+ Matrix66S link5_AI;
+ Velocity link5_a;
+ Velocity link5_v;
+ Velocity link5_c;
+ Force link5_p;
+
+ Column6DS link5_U;
+ SCALAR link5_D;
+ SCALAR link5_u;
+ // Link 'link6' :
+ Matrix66S link6_AI;
+ Velocity link6_a;
+ Velocity link6_v;
+ Velocity link6_c;
+ Force link6_p;
+
+ Column6DS link6_U;
+ SCALAR link6_D;
+ SCALAR link6_u;
+private:
+ static const ExtForces zeroExtForces;
+};
+
+template <typename TRAIT>
+inline void ForwardDynamics<TRAIT>::setJointStatus(const JointState& q) const {
+ (motionTransforms-> fr_link1_X_fr_link0)(q);
+ (motionTransforms-> fr_link2_X_fr_link1)(q);
+ (motionTransforms-> fr_link3_X_fr_link2)(q);
+ (motionTransforms-> fr_link4_X_fr_link3)(q);
+ (motionTransforms-> fr_link5_X_fr_link4)(q);
+ (motionTransforms-> fr_link6_X_fr_link5)(q);
+}
+
+template<typename TRAIT>
+inline void ForwardDynamics<TRAIT>::fd(
+ JointState& qdd,
+ const JointState& q,
+ const JointState& qd,
+ const JointState& tau,
+ const ExtForces& fext/* = zeroExtForces */)
+{
+ setJointStatus(q);
+ fd(qdd, qd, tau, fext);
+}
+
+} // namespace tpl
+
+typedef tpl::ForwardDynamics<rbd::DoubleTrait> ForwardDynamics;
+
+}
+}
+}
+
+#include "forward_dynamics.impl.h"
+
+#endif
diff --git a/ct_rbd/test/models/testIrb4600/generated/forward_dynamics.impl.h b/ct_rbd/test/models/testIrb4600/generated/forward_dynamics.impl.h
new file mode 100644
index 0000000..a336d80
--- /dev/null
+++ b/ct_rbd/test/models/testIrb4600/generated/forward_dynamics.impl.h
@@ -0,0 +1,217 @@
+
+
+// Initialization of static-const data
+template<typename TRAIT>
+const typename iit::testirb4600::dyn::tpl::ForwardDynamics<TRAIT>::ExtForces
+ iit::testirb4600::dyn::tpl::ForwardDynamics<TRAIT>::zeroExtForces(Force::Zero());
+
+template<typename TRAIT>
+iit::testirb4600::dyn::tpl::ForwardDynamics<TRAIT>::ForwardDynamics(iit::testirb4600::dyn::tpl::InertiaProperties<TRAIT>& inertia, MTransforms& transforms) :
+ inertiaProps( & inertia ),
+ motionTransforms( & transforms )
+{
+ link1_v.setZero();
+ link1_c.setZero();
+ link2_v.setZero();
+ link2_c.setZero();
+ link3_v.setZero();
+ link3_c.setZero();
+ link4_v.setZero();
+ link4_c.setZero();
+ link5_v.setZero();
+ link5_c.setZero();
+ link6_v.setZero();
+ link6_c.setZero();
+
+ vcross.setZero();
+ Ia_r.setZero();
+
+}
+
+template <typename TRAIT>
+void iit::testirb4600::dyn::tpl::ForwardDynamics<TRAIT>::fd(
+ JointState& qdd,
+ const JointState& qd,
+ const JointState& tau,
+ const ExtForces& fext/* = zeroExtForces */)
+{
+
+ link1_AI = inertiaProps->getTensor_link1();
+ link1_p = - fext[LINK1];
+ link2_AI = inertiaProps->getTensor_link2();
+ link2_p = - fext[LINK2];
+ link3_AI = inertiaProps->getTensor_link3();
+ link3_p = - fext[LINK3];
+ link4_AI = inertiaProps->getTensor_link4();
+ link4_p = - fext[LINK4];
+ link5_AI = inertiaProps->getTensor_link5();
+ link5_p = - fext[LINK5];
+ link6_AI = inertiaProps->getTensor_link6();
+ link6_p = - fext[LINK6];
+ // ---------------------- FIRST PASS ---------------------- //
+ // Note that, during the first pass, the articulated inertias are really
+ // just the spatial inertia of the links (see assignments above).
+ // Afterwards things change, and articulated inertias shall not be used
+ // in functions which work specifically with spatial inertias.
+
+ // + Link link1
+ // - The spatial velocity:
+ link1_v(rbd::AZ) = qd(JA);
+
+ // - The bias force term:
+ link1_p += iit::rbd::vxIv(qd(JA), link1_AI);
+
+ // + Link link2
+ // - The spatial velocity:
+ link2_v = (motionTransforms-> fr_link2_X_fr_link1) * link1_v;
+ link2_v(rbd::AZ) += qd(JB);
+
+ // - The velocity-product acceleration term:
+ iit::rbd::motionCrossProductMx<SCALAR>(link2_v, vcross);
+ link2_c = vcross.col(rbd::AZ) * qd(JB);
+
+ // - The bias force term:
+ link2_p += iit::rbd::vxIv(link2_v, link2_AI);
+
+ // + Link link3
+ // - The spatial velocity:
+ link3_v = (motionTransforms-> fr_link3_X_fr_link2) * link2_v;
+ link3_v(rbd::AZ) += qd(JC);
+
+ // - The velocity-product acceleration term:
+ iit::rbd::motionCrossProductMx<SCALAR>(link3_v, vcross);
+ link3_c = vcross.col(rbd::AZ) * qd(JC);
+
+ // - The bias force term:
+ link3_p += iit::rbd::vxIv(link3_v, link3_AI);
+
+ // + Link link4
+ // - The spatial velocity:
+ link4_v = (motionTransforms-> fr_link4_X_fr_link3) * link3_v;
+ link4_v(rbd::AZ) += qd(JD);
+
+ // - The velocity-product acceleration term:
+ iit::rbd::motionCrossProductMx<SCALAR>(link4_v, vcross);
+ link4_c = vcross.col(rbd::AZ) * qd(JD);
+
+ // - The bias force term:
+ link4_p += iit::rbd::vxIv(link4_v, link4_AI);
+
+ // + Link link5
+ // - The spatial velocity:
+ link5_v = (motionTransforms-> fr_link5_X_fr_link4) * link4_v;
+ link5_v(rbd::AZ) += qd(JE);
+
+ // - The velocity-product acceleration term:
+ iit::rbd::motionCrossProductMx<SCALAR>(link5_v, vcross);
+ link5_c = vcross.col(rbd::AZ) * qd(JE);
+
+ // - The bias force term:
+ link5_p += iit::rbd::vxIv(link5_v, link5_AI);
+
+ // + Link link6
+ // - The spatial velocity:
+ link6_v = (motionTransforms-> fr_link6_X_fr_link5) * link5_v;
+ link6_v(rbd::AZ) += qd(JF);
+
+ // - The velocity-product acceleration term:
+ iit::rbd::motionCrossProductMx<SCALAR>(link6_v, vcross);
+ link6_c = vcross.col(rbd::AZ) * qd(JF);
+
+ // - The bias force term:
+ link6_p += iit::rbd::vxIv(link6_v, link6_AI);
+
+
+ // ---------------------- SECOND PASS ---------------------- //
+ Matrix66S IaB;
+ Force pa;
+
+ // + Link link6
+ link6_u = tau(JF) - link6_p(rbd::AZ);
+ link6_U = link6_AI.col(rbd::AZ);
+ link6_D = link6_U(rbd::AZ);
+
+ iit::rbd::compute_Ia_revolute(link6_AI, link6_U, link6_D, Ia_r); // same as: Ia_r = link6_AI - link6_U/link6_D * link6_U.transpose();
+ pa = link6_p + Ia_r * link6_c + link6_U * link6_u/link6_D;
+ ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_link6_X_fr_link5, IaB);
+ link5_AI += IaB;
+ link5_p += (motionTransforms-> fr_link6_X_fr_link5).transpose() * pa;
+
+ // + Link link5
+ link5_u = tau(JE) - link5_p(rbd::AZ);
+ link5_U = link5_AI.col(rbd::AZ);
+ link5_D = link5_U(rbd::AZ);
+
+ iit::rbd::compute_Ia_revolute(link5_AI, link5_U, link5_D, Ia_r); // same as: Ia_r = link5_AI - link5_U/link5_D * link5_U.transpose();
+ pa = link5_p + Ia_r * link5_c + link5_U * link5_u/link5_D;
+ ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_link5_X_fr_link4, IaB);
+ link4_AI += IaB;
+ link4_p += (motionTransforms-> fr_link5_X_fr_link4).transpose() * pa;
+
+ // + Link link4
+ link4_u = tau(JD) - link4_p(rbd::AZ);
+ link4_U = link4_AI.col(rbd::AZ);
+ link4_D = link4_U(rbd::AZ);
+
+ iit::rbd::compute_Ia_revolute(link4_AI, link4_U, link4_D, Ia_r); // same as: Ia_r = link4_AI - link4_U/link4_D * link4_U.transpose();
+ pa = link4_p + Ia_r * link4_c + link4_U * link4_u/link4_D;
+ ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_link4_X_fr_link3, IaB);
+ link3_AI += IaB;
+ link3_p += (motionTransforms-> fr_link4_X_fr_link3).transpose() * pa;
+
+ // + Link link3
+ link3_u = tau(JC) - link3_p(rbd::AZ);
+ link3_U = link3_AI.col(rbd::AZ);
+ link3_D = link3_U(rbd::AZ);
+
+ iit::rbd::compute_Ia_revolute(link3_AI, link3_U, link3_D, Ia_r); // same as: Ia_r = link3_AI - link3_U/link3_D * link3_U.transpose();
+ pa = link3_p + Ia_r * link3_c + link3_U * link3_u/link3_D;
+ ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_link3_X_fr_link2, IaB);
+ link2_AI += IaB;
+ link2_p += (motionTransforms-> fr_link3_X_fr_link2).transpose() * pa;
+
+ // + Link link2
+ link2_u = tau(JB) - link2_p(rbd::AZ);
+ link2_U = link2_AI.col(rbd::AZ);
+ link2_D = link2_U(rbd::AZ);
+
+ iit::rbd::compute_Ia_revolute(link2_AI, link2_U, link2_D, Ia_r); // same as: Ia_r = link2_AI - link2_U/link2_D * link2_U.transpose();
+ pa = link2_p + Ia_r * link2_c + link2_U * link2_u/link2_D;
+ ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_link2_X_fr_link1, IaB);
+ link1_AI += IaB;
+ link1_p += (motionTransforms-> fr_link2_X_fr_link1).transpose() * pa;
+
+ // + Link link1
+ link1_u = tau(JA) - link1_p(rbd::AZ);
+ link1_U = link1_AI.col(rbd::AZ);
+ link1_D = link1_U(rbd::AZ);
+
+
+
+ // ---------------------- THIRD PASS ---------------------- //
+ link1_a = (motionTransforms-> fr_link1_X_fr_link0).col(rbd::LZ) * SCALAR(iit::rbd::g);
+ qdd(JA) = (link1_u - link1_U.dot(link1_a)) / link1_D;
+ link1_a(rbd::AZ) += qdd(JA);
+
+ link2_a = (motionTransforms-> fr_link2_X_fr_link1) * link1_a + link2_c;
+ qdd(JB) = (link2_u - link2_U.dot(link2_a)) / link2_D;
+ link2_a(rbd::AZ) += qdd(JB);
+
+ link3_a = (motionTransforms-> fr_link3_X_fr_link2) * link2_a + link3_c;
+ qdd(JC) = (link3_u - link3_U.dot(link3_a)) / link3_D;
+ link3_a(rbd::AZ) += qdd(JC);
+
+ link4_a = (motionTransforms-> fr_link4_X_fr_link3) * link3_a + link4_c;
+ qdd(JD) = (link4_u - link4_U.dot(link4_a)) / link4_D;
+ link4_a(rbd::AZ) += qdd(JD);
+
+ link5_a = (motionTransforms-> fr_link5_X_fr_link4) * link4_a + link5_c;
+ qdd(JE) = (link5_u - link5_U.dot(link5_a)) / link5_D;
+ link5_a(rbd::AZ) += qdd(JE);
+
+ link6_a = (motionTransforms-> fr_link6_X_fr_link5) * link5_a + link6_c;
+ qdd(JF) = (link6_u - link6_U.dot(link6_a)) / link6_D;
+ link6_a(rbd::AZ) += qdd(JF);
+
+
+}
diff --git a/ct_rbd/test/models/testIrb4600/generated/inertia_properties.h b/ct_rbd/test/models/testIrb4600/generated/inertia_properties.h
new file mode 100644
index 0000000..22371a6
--- /dev/null
+++ b/ct_rbd/test/models/testIrb4600/generated/inertia_properties.h
@@ -0,0 +1,163 @@
+#ifndef IIT_ROBOT_TESTIRB4600_INERTIA_PROPERTIES_H_
+#define IIT_ROBOT_TESTIRB4600_INERTIA_PROPERTIES_H_
+
+#include <Eigen/Dense>
+#include <iit/rbd/rbd.h>
+#include <iit/rbd/InertiaMatrix.h>
+#include <iit/rbd/utils.h>
+#include <iit/rbd/traits/DoubleTrait.h>
+
+#include "declarations.h"
+
+namespace iit {
+namespace testirb4600 {
+/**
+ * This namespace encloses classes and functions related to the Dynamics
+ * of the robot testirb4600.
+ */
+namespace dyn {
+
+using InertiaMatrix = iit::rbd::InertiaMatrixDense;
+
+namespace tpl{
+
+template<typename TRAIT>
+class InertiaProperties {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef typename TRAIT::Scalar SCALAR;
+
+ typedef iit::rbd::tpl::InertiaMatrixDense<SCALAR> IMatrix;
+ typedef Eigen::Matrix<SCALAR, 3, 1> Vec3d;
+
+ InertiaProperties();
+ ~InertiaProperties();
+ const IMatrix& getTensor_link1() const;
+ const IMatrix& getTensor_link2() const;
+ const IMatrix& getTensor_link3() const;
+ const IMatrix& getTensor_link4() const;
+ const IMatrix& getTensor_link5() const;
+ const IMatrix& getTensor_link6() const;
+ SCALAR getMass_link1() const;
+ SCALAR getMass_link2() const;
+ SCALAR getMass_link3() const;
+ SCALAR getMass_link4() const;
+ SCALAR getMass_link5() const;
+ SCALAR getMass_link6() const;
+ const Vec3d& getCOM_link1() const;
+ const Vec3d& getCOM_link2() const;
+ const Vec3d& getCOM_link3() const;
+ const Vec3d& getCOM_link4() const;
+ const Vec3d& getCOM_link5() const;
+ const Vec3d& getCOM_link6() const;
+ SCALAR getTotalMass() const;
+
+ private:
+
+ IMatrix tensor_link1;
+ IMatrix tensor_link2;
+ IMatrix tensor_link3;
+ IMatrix tensor_link4;
+ IMatrix tensor_link5;
+ IMatrix tensor_link6;
+ Vec3d com_link1;
+ Vec3d com_link2;
+ Vec3d com_link3;
+ Vec3d com_link4;
+ Vec3d com_link5;
+ Vec3d com_link6;
+};
+
+template <typename TRAIT>
+inline InertiaProperties<TRAIT>::~InertiaProperties() {}
+
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::IMatrix& InertiaProperties<TRAIT>::getTensor_link1() const {
+ return this->tensor_link1;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::IMatrix& InertiaProperties<TRAIT>::getTensor_link2() const {
+ return this->tensor_link2;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::IMatrix& InertiaProperties<TRAIT>::getTensor_link3() const {
+ return this->tensor_link3;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::IMatrix& InertiaProperties<TRAIT>::getTensor_link4() const {
+ return this->tensor_link4;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::IMatrix& InertiaProperties<TRAIT>::getTensor_link5() const {
+ return this->tensor_link5;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::IMatrix& InertiaProperties<TRAIT>::getTensor_link6() const {
+ return this->tensor_link6;
+}
+template <typename TRAIT>
+inline typename InertiaProperties<TRAIT>::SCALAR InertiaProperties<TRAIT>::getMass_link1() const {
+ return this->tensor_link1.getMass();
+}
+template <typename TRAIT>
+inline typename InertiaProperties<TRAIT>::SCALAR InertiaProperties<TRAIT>::getMass_link2() const {
+ return this->tensor_link2.getMass();
+}
+template <typename TRAIT>
+inline typename InertiaProperties<TRAIT>::SCALAR InertiaProperties<TRAIT>::getMass_link3() const {
+ return this->tensor_link3.getMass();
+}
+template <typename TRAIT>
+inline typename InertiaProperties<TRAIT>::SCALAR InertiaProperties<TRAIT>::getMass_link4() const {
+ return this->tensor_link4.getMass();
+}
+template <typename TRAIT>
+inline typename InertiaProperties<TRAIT>::SCALAR InertiaProperties<TRAIT>::getMass_link5() const {
+ return this->tensor_link5.getMass();
+}
+template <typename TRAIT>
+inline typename InertiaProperties<TRAIT>::SCALAR InertiaProperties<TRAIT>::getMass_link6() const {
+ return this->tensor_link6.getMass();
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::Vec3d& InertiaProperties<TRAIT>::getCOM_link1() const {
+ return this->com_link1;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::Vec3d& InertiaProperties<TRAIT>::getCOM_link2() const {
+ return this->com_link2;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::Vec3d& InertiaProperties<TRAIT>::getCOM_link3() const {
+ return this->com_link3;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::Vec3d& InertiaProperties<TRAIT>::getCOM_link4() const {
+ return this->com_link4;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::Vec3d& InertiaProperties<TRAIT>::getCOM_link5() const {
+ return this->com_link5;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::Vec3d& InertiaProperties<TRAIT>::getCOM_link6() const {
+ return this->com_link6;
+}
+
+template <typename TRAIT>
+inline typename InertiaProperties<TRAIT>::SCALAR InertiaProperties<TRAIT>::getTotalMass() const {
+ return 120.0 + 120.0 + 120.0 + 40.0 + 10.0 + 5.0;
+}
+
+} // namespace tpl
+
+using InertiaProperties = tpl::InertiaProperties<rbd::DoubleTrait>;
+
+}
+}
+}
+
+#include "inertia_properties.impl.h"
+
+#endif
diff --git a/ct_rbd/test/models/testIrb4600/generated/inertia_properties.impl.h b/ct_rbd/test/models/testIrb4600/generated/inertia_properties.impl.h
new file mode 100644
index 0000000..47f1b87
--- /dev/null
+++ b/ct_rbd/test/models/testIrb4600/generated/inertia_properties.impl.h
@@ -0,0 +1,77 @@
+template <typename TRAIT>
+iit::testirb4600::dyn::tpl::InertiaProperties<TRAIT>::InertiaProperties()
+{
+ com_link1 = iit::rbd::Vector3d(0.09,0.0,0.25).cast<SCALAR>();
+ tensor_link1.fill(
+ SCALAR(120.0),
+ com_link1,
+ rbd::Utils::buildInertiaTensor(
+ SCALAR(10.02),
+ SCALAR(10.992001),
+ SCALAR(2.7719998),
+ SCALAR(0.0),
+ SCALAR(2.7),
+ SCALAR(0.0)) );
+
+ com_link2 = iit::rbd::Vector3d(0.47,0.04,-0.17).cast<SCALAR>();
+ tensor_link2.fill(
+ SCALAR(120.0),
+ com_link2,
+ rbd::Utils::buildInertiaTensor(
+ SCALAR(4.36),
+ SCALAR(41.225998),
+ SCALAR(38.449997),
+ SCALAR(2.256),
+ SCALAR(-9.588),
+ SCALAR(-0.816)) );
+
+ com_link3 = iit::rbd::Vector3d(0.035,-0.133,0.044).cast<SCALAR>();
+ tensor_link3.fill(
+ SCALAR(120.0),
+ com_link3,
+ rbd::Utils::buildInertiaTensor(
+ SCALAR(6.0550003),
+ SCALAR(5.4093204),
+ SCALAR(7.29968),
+ SCALAR(-0.5586),
+ SCALAR(0.1848),
+ SCALAR(-0.70224)) );
+
+ com_link4 = iit::rbd::Vector3d(0.0,0.0,0.5).cast<SCALAR>();
+ tensor_link4.fill(
+ SCALAR(40.0),
+ com_link4,
+ rbd::Utils::buildInertiaTensor(
+ SCALAR(12.0),
+ SCALAR(12.0),
+ SCALAR(0.482),
+ SCALAR(0.0),
+ SCALAR(0.0),
+ SCALAR(0.0)) );
+
+ com_link5 = iit::rbd::Vector3d(0.07,0.0,0.0).cast<SCALAR>();
+ tensor_link5.fill(
+ SCALAR(10.0),
+ com_link5,
+ rbd::Utils::buildInertiaTensor(
+ SCALAR(0.04),
+ SCALAR(0.089),
+ SCALAR(0.094000004),
+ SCALAR(0.0),
+ SCALAR(0.0),
+ SCALAR(0.0)) );
+
+ com_link6 = iit::rbd::Vector3d(0.081,-0.091,0.232).cast<SCALAR>();
+ tensor_link6.fill(
+ SCALAR(5.0),
+ com_link6,
+ rbd::Utils::buildInertiaTensor(
+ SCALAR(0.014),
+ SCALAR(0.014),
+ SCALAR(0.028),
+ SCALAR(0.0),
+ SCALAR(0.0),
+ SCALAR(0.0)) );
+
+}
+
diff --git a/ct_rbd/test/models/testIrb4600/generated/inverse_dynamics.h b/ct_rbd/test/models/testIrb4600/generated/inverse_dynamics.h
new file mode 100644
index 0000000..07450c5
--- /dev/null
+++ b/ct_rbd/test/models/testIrb4600/generated/inverse_dynamics.h
@@ -0,0 +1,239 @@
+#ifndef IIT_TESTIRB4600_INVERSE_DYNAMICS_H_
+#define IIT_TESTIRB4600_INVERSE_DYNAMICS_H_
+
+#include <Eigen/Dense>
+#include <iit/rbd/rbd.h>
+#include <iit/rbd/InertiaMatrix.h>
+#include <iit/rbd/utils.h>
+#include <iit/rbd/robcogen_commons.h>
+#include <iit/rbd/traits/DoubleTrait.h>
+
+#include "declarations.h"
+#include "inertia_properties.h"
+#include "transforms.h"
+#include "link_data_map.h"
+
+namespace iit {
+namespace testirb4600 {
+namespace dyn {
+
+/**
+ * The Inverse Dynamics routine for the robot testirb4600.
+ *
+ * In addition to the full Newton-Euler algorithm, specialized versions to compute
+ * only certain terms are provided.
+ * The parameters common to most of the methods are the joint status vector \c q, the
+ * joint velocity vector \c qd and the acceleration vector \c qdd.
+ *
+ * Additional overloaded methods are provided without the \c q parameter. These
+ * methods use the current configuration of the robot; they are provided for the
+ * sake of efficiency, in case the motion transforms of the robot have already
+ * been updated elsewhere with the most recent configuration (eg by a call to
+ * setJointStatus()), so that it is useless to compute them again.
+ *
+ * Whenever present, the external forces parameter is a set of external
+ * wrenches acting on the robot links. Each wrench must be expressed in
+ * the reference frame of the link it is excerted on.
+ */
+
+namespace tpl {
+
+template <typename TRAIT>
+class InverseDynamics {
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef typename TRAIT::Scalar SCALAR;
+
+ typedef iit::rbd::Core<SCALAR> CoreS;
+
+ typedef typename CoreS::ForceVector Force;
+ typedef LinkDataMap<Force> ExtForces;
+ typedef typename CoreS::VelocityVector Velocity;
+ typedef typename CoreS::VelocityVector Acceleration;
+ typedef iit::rbd::tpl::InertiaMatrixDense<SCALAR> InertiaMatrix;
+ typedef iit::testirb4600::tpl::JointState<SCALAR> JointState;
+ typedef typename CoreS::Matrix66 Matrix66s;
+ typedef iit::testirb4600::tpl::MotionTransforms<TRAIT> MTransforms;
+ typedef InertiaProperties<TRAIT> IProperties;
+
+public:
+ /**
+ * Default constructor
+ * \param in the inertia properties of the links
+ * \param tr the container of all the spatial motion transforms of
+ * the robot testirb4600, which will be used by this instance
+ * to compute inverse-dynamics.
+ */
+ InverseDynamics(IProperties& in, MTransforms& tr);
+
+ /** \name Inverse dynamics
+ * The full Newton-Euler algorithm for the inverse dynamics of this robot.
+ *
+ * \param[out] jForces the joint force vector required to achieve the desired accelerations
+ * \param[in] q the joint position vector
+ * \param[in] qd the joint velocity vector
+ * \param[in] qdd the desired joint acceleration vector
+ * \param[in] fext the external forces acting on the links; this parameters
+ * defaults to zero
+ */
+ ///@{
+ void id(
+ JointState& jForces,
+ const JointState& q, const JointState& qd, const JointState& qdd,
+ const ExtForces& fext = zeroExtForces);
+ void id(
+ JointState& jForces,
+ const JointState& qd, const JointState& qdd,
+ const ExtForces& fext = zeroExtForces);
+ ///@}
+
+ /** \name Gravity terms
+ * The joint forces (linear or rotational) required to compensate
+ * for the effect of gravity, in a specific configuration.
+ */
+ ///@{
+ void G_terms(JointState& jForces, const JointState& q);
+ void G_terms(JointState& jForces);
+ ///@}
+
+ /** \name Centrifugal and Coriolis terms
+ * The forces (linear or rotational) acting on the joints due to centrifugal and
+ * Coriolis effects, for a specific configuration.
+ */
+ ///@{
+ void C_terms(JointState& jForces, const JointState& q, const JointState& qd);
+ void C_terms(JointState& jForces, const JointState& qd);
+ ///@}
+ /** Updates all the kinematics transforms used by the inverse dynamics routine. */
+ void setJointStatus(const JointState& q) const;
+
+public:
+ /** \name Getters
+ * These functions return various spatial quantities used internally
+ * by the inverse dynamics routines, like the spatial acceleration
+ * of the links.
+ *
+ * The getters can be useful to retrieve the additional data that is not
+ * returned explicitly by the inverse dynamics routines even though it
+ * is computed. For example, after a call to the inverse dynamics,
+ * the spatial velocity of all the links has been determined and
+ * can be accessed.
+ *
+ * However, beware that certain routines might not use some of the
+ * spatial quantities, which therefore would retain their last value
+ * without being updated nor reset (for example, the spatial velocity
+ * of the links is unaffected by the computation of the gravity terms).
+ */
+ ///@{
+ const Velocity& getVelocity_link1() const { return link1_v; }
+ const Acceleration& getAcceleration_link1() const { return link1_a; }
+ const Force& getForce_link1() const { return link1_f; }
+ const Velocity& getVelocity_link2() const { return link2_v; }
+ const Acceleration& getAcceleration_link2() const { return link2_a; }
+ const Force& getForce_link2() const { return link2_f; }
+ const Velocity& getVelocity_link3() const { return link3_v; }
+ const Acceleration& getAcceleration_link3() const { return link3_a; }
+ const Force& getForce_link3() const { return link3_f; }
+ const Velocity& getVelocity_link4() const { return link4_v; }
+ const Acceleration& getAcceleration_link4() const { return link4_a; }
+ const Force& getForce_link4() const { return link4_f; }
+ const Velocity& getVelocity_link5() const { return link5_v; }
+ const Acceleration& getAcceleration_link5() const { return link5_a; }
+ const Force& getForce_link5() const { return link5_f; }
+ const Velocity& getVelocity_link6() const { return link6_v; }
+ const Acceleration& getAcceleration_link6() const { return link6_a; }
+ const Force& getForce_link6() const { return link6_f; }
+ ///@}
+protected:
+ void firstPass(const JointState& qd, const JointState& qdd, const ExtForces& fext);
+ void secondPass(JointState& jForces);
+
+private:
+ IProperties* inertiaProps;
+ MTransforms* xm;
+private:
+ Matrix66s vcross; // support variable
+ // Link 'link1' :
+ const InertiaMatrix& link1_I;
+ Velocity link1_v;
+ Acceleration link1_a;
+ Force link1_f;
+ // Link 'link2' :
+ const InertiaMatrix& link2_I;
+ Velocity link2_v;
+ Acceleration link2_a;
+ Force link2_f;
+ // Link 'link3' :
+ const InertiaMatrix& link3_I;
+ Velocity link3_v;
+ Acceleration link3_a;
+ Force link3_f;
+ // Link 'link4' :
+ const InertiaMatrix& link4_I;
+ Velocity link4_v;
+ Acceleration link4_a;
+ Force link4_f;
+ // Link 'link5' :
+ const InertiaMatrix& link5_I;
+ Velocity link5_v;
+ Acceleration link5_a;
+ Force link5_f;
+ // Link 'link6' :
+ const InertiaMatrix& link6_I;
+ Velocity link6_v;
+ Acceleration link6_a;
+ Force link6_f;
+
+
+private:
+ static const ExtForces zeroExtForces;
+};
+
+template <typename TRAIT>
+inline void InverseDynamics<TRAIT>::setJointStatus(const JointState& q) const
+{
+ (xm->fr_link1_X_fr_link0)(q);
+ (xm->fr_link2_X_fr_link1)(q);
+ (xm->fr_link3_X_fr_link2)(q);
+ (xm->fr_link4_X_fr_link3)(q);
+ (xm->fr_link5_X_fr_link4)(q);
+ (xm->fr_link6_X_fr_link5)(q);
+}
+
+template <typename TRAIT>
+inline void InverseDynamics<TRAIT>::G_terms(JointState& jForces, const JointState& q)
+{
+ setJointStatus(q);
+ G_terms(jForces);
+}
+
+template <typename TRAIT>
+inline void InverseDynamics<TRAIT>::C_terms(JointState& jForces, const JointState& q, const JointState& qd)
+{
+ setJointStatus(q);
+ C_terms(jForces, qd);
+}
+
+template <typename TRAIT>
+inline void InverseDynamics<TRAIT>::id(
+ JointState& jForces,
+ const JointState& q, const JointState& qd, const JointState& qdd,
+ const ExtForces& fext)
+{
+ setJointStatus(q);
+ id(jForces, qd, qdd, fext);
+}
+
+} // namespace tpl
+
+typedef tpl::InverseDynamics<rbd::DoubleTrait> InverseDynamics;
+
+}
+}
+
+}
+
+#include "inverse_dynamics.impl.h"
+
+#endif
diff --git a/ct_rbd/test/models/testIrb4600/generated/inverse_dynamics.impl.h b/ct_rbd/test/models/testIrb4600/generated/inverse_dynamics.impl.h
new file mode 100644
index 0000000..60fd939
--- /dev/null
+++ b/ct_rbd/test/models/testIrb4600/generated/inverse_dynamics.impl.h
@@ -0,0 +1,216 @@
+
+// Initialization of static-const data
+template <typename TRAIT>
+const typename iit::testirb4600::dyn::tpl::InverseDynamics<TRAIT>::ExtForces
+iit::testirb4600::dyn::tpl::InverseDynamics<TRAIT>::zeroExtForces(Force::Zero());
+
+template <typename TRAIT>
+iit::testirb4600::dyn::tpl::InverseDynamics<TRAIT>::InverseDynamics(IProperties& inertia, MTransforms& transforms) :
+ inertiaProps( & inertia ),
+ xm( & transforms ),
+ link1_I(inertiaProps->getTensor_link1() ),
+ link2_I(inertiaProps->getTensor_link2() ),
+ link3_I(inertiaProps->getTensor_link3() ),
+ link4_I(inertiaProps->getTensor_link4() ),
+ link5_I(inertiaProps->getTensor_link5() ),
+ link6_I(inertiaProps->getTensor_link6() )
+ {
+#ifndef EIGEN_NO_DEBUG
+ std::cout << "Robot testirb4600, InverseDynamics<TRAIT>::InverseDynamics()" << std::endl;
+ std::cout << "Compiled with Eigen debug active" << std::endl;
+#endif
+ link1_v.setZero();
+ link2_v.setZero();
+ link3_v.setZero();
+ link4_v.setZero();
+ link5_v.setZero();
+ link6_v.setZero();
+
+ vcross.setZero();
+}
+
+template <typename TRAIT>
+void iit::testirb4600::dyn::tpl::InverseDynamics<TRAIT>::id(
+ JointState& jForces,
+ const JointState& qd, const JointState& qdd,
+ const ExtForces& fext)
+{
+ firstPass(qd, qdd, fext);
+ secondPass(jForces);
+}
+
+template <typename TRAIT>
+void iit::testirb4600::dyn::tpl::InverseDynamics<TRAIT>::G_terms(JointState& jForces)
+{
+ // Link 'link1'
+ link1_a = (xm->fr_link1_X_fr_link0).col(iit::rbd::LZ) * SCALAR(iit::rbd::g);
+ link1_f = link1_I * link1_a;
+ // Link 'link2'
+ link2_a = (xm->fr_link2_X_fr_link1) * link1_a;
+ link2_f = link2_I * link2_a;
+ // Link 'link3'
+ link3_a = (xm->fr_link3_X_fr_link2) * link2_a;
+ link3_f = link3_I * link3_a;
+ // Link 'link4'
+ link4_a = (xm->fr_link4_X_fr_link3) * link3_a;
+ link4_f = link4_I * link4_a;
+ // Link 'link5'
+ link5_a = (xm->fr_link5_X_fr_link4) * link4_a;
+ link5_f = link5_I * link5_a;
+ // Link 'link6'
+ link6_a = (xm->fr_link6_X_fr_link5) * link5_a;
+ link6_f = link6_I * link6_a;
+
+ secondPass(jForces);
+}
+
+template <typename TRAIT>
+void iit::testirb4600::dyn::tpl::InverseDynamics<TRAIT>::C_terms(JointState& jForces, const JointState& qd)
+{
+ // Link 'link1'
+ link1_v(iit::rbd::AZ) = qd(JA); // link1_v = vJ, for the first link of a fixed base robot
+
+ link1_f = iit::rbd::vxIv(qd(JA), link1_I);
+
+ // Link 'link2'
+ link2_v = ((xm->fr_link2_X_fr_link1) * link1_v);
+ link2_v(iit::rbd::AZ) += qd(JB);
+
+ iit::rbd::motionCrossProductMx<SCALAR>(link2_v, vcross);
+
+ link2_a = (vcross.col(iit::rbd::AZ) * qd(JB));
+
+ link2_f = link2_I * link2_a + iit::rbd::vxIv(link2_v, link2_I);
+
+ // Link 'link3'
+ link3_v = ((xm->fr_link3_X_fr_link2) * link2_v);
+ link3_v(iit::rbd::AZ) += qd(JC);
+
+ iit::rbd::motionCrossProductMx<SCALAR>(link3_v, vcross);
+
+ link3_a = (xm->fr_link3_X_fr_link2) * link2_a + vcross.col(iit::rbd::AZ) * qd(JC);
+
+ link3_f = link3_I * link3_a + iit::rbd::vxIv(link3_v, link3_I);
+
+ // Link 'link4'
+ link4_v = ((xm->fr_link4_X_fr_link3) * link3_v);
+ link4_v(iit::rbd::AZ) += qd(JD);
+
+ iit::rbd::motionCrossProductMx<SCALAR>(link4_v, vcross);
+
+ link4_a = (xm->fr_link4_X_fr_link3) * link3_a + vcross.col(iit::rbd::AZ) * qd(JD);
+
+ link4_f = link4_I * link4_a + iit::rbd::vxIv(link4_v, link4_I);
+
+ // Link 'link5'
+ link5_v = ((xm->fr_link5_X_fr_link4) * link4_v);
+ link5_v(iit::rbd::AZ) += qd(JE);
+
+ iit::rbd::motionCrossProductMx<SCALAR>(link5_v, vcross);
+
+ link5_a = (xm->fr_link5_X_fr_link4) * link4_a + vcross.col(iit::rbd::AZ) * qd(JE);
+
+ link5_f = link5_I * link5_a + iit::rbd::vxIv(link5_v, link5_I);
+
+ // Link 'link6'
+ link6_v = ((xm->fr_link6_X_fr_link5) * link5_v);
+ link6_v(iit::rbd::AZ) += qd(JF);
+
+ iit::rbd::motionCrossProductMx<SCALAR>(link6_v, vcross);
+
+ link6_a = (xm->fr_link6_X_fr_link5) * link5_a + vcross.col(iit::rbd::AZ) * qd(JF);
+
+ link6_f = link6_I * link6_a + iit::rbd::vxIv(link6_v, link6_I);
+
+
+ secondPass(jForces);
+}
+
+template <typename TRAIT>
+void iit::testirb4600::dyn::tpl::InverseDynamics<TRAIT>::firstPass(const JointState& qd, const JointState& qdd, const ExtForces& fext)
+{
+ // First pass, link 'link1'
+ link1_a = (xm->fr_link1_X_fr_link0).col(iit::rbd::LZ) * SCALAR(iit::rbd::g);
+ link1_a(iit::rbd::AZ) += qdd(JA);
+ link1_v(iit::rbd::AZ) = qd(JA); // link1_v = vJ, for the first link of a fixed base robot
+
+ link1_f = link1_I * link1_a + iit::rbd::vxIv(qd(JA), link1_I) - fext[LINK1];
+
+ // First pass, link 'link2'
+ link2_v = ((xm->fr_link2_X_fr_link1) * link1_v);
+ link2_v(iit::rbd::AZ) += qd(JB);
+
+ iit::rbd::motionCrossProductMx<SCALAR>(link2_v, vcross);
+
+ link2_a = (xm->fr_link2_X_fr_link1) * link1_a + vcross.col(iit::rbd::AZ) * qd(JB);
+ link2_a(iit::rbd::AZ) += qdd(JB);
+
+ link2_f = link2_I * link2_a + iit::rbd::vxIv(link2_v, link2_I) - fext[LINK2];
+
+ // First pass, link 'link3'
+ link3_v = ((xm->fr_link3_X_fr_link2) * link2_v);
+ link3_v(iit::rbd::AZ) += qd(JC);
+
+ iit::rbd::motionCrossProductMx<SCALAR>(link3_v, vcross);
+
+ link3_a = (xm->fr_link3_X_fr_link2) * link2_a + vcross.col(iit::rbd::AZ) * qd(JC);
+ link3_a(iit::rbd::AZ) += qdd(JC);
+
+ link3_f = link3_I * link3_a + iit::rbd::vxIv(link3_v, link3_I) - fext[LINK3];
+
+ // First pass, link 'link4'
+ link4_v = ((xm->fr_link4_X_fr_link3) * link3_v);
+ link4_v(iit::rbd::AZ) += qd(JD);
+
+ iit::rbd::motionCrossProductMx<SCALAR>(link4_v, vcross);
+
+ link4_a = (xm->fr_link4_X_fr_link3) * link3_a + vcross.col(iit::rbd::AZ) * qd(JD);
+ link4_a(iit::rbd::AZ) += qdd(JD);
+
+ link4_f = link4_I * link4_a + iit::rbd::vxIv(link4_v, link4_I) - fext[LINK4];
+
+ // First pass, link 'link5'
+ link5_v = ((xm->fr_link5_X_fr_link4) * link4_v);
+ link5_v(iit::rbd::AZ) += qd(JE);
+
+ iit::rbd::motionCrossProductMx<SCALAR>(link5_v, vcross);
+
+ link5_a = (xm->fr_link5_X_fr_link4) * link4_a + vcross.col(iit::rbd::AZ) * qd(JE);
+ link5_a(iit::rbd::AZ) += qdd(JE);
+
+ link5_f = link5_I * link5_a + iit::rbd::vxIv(link5_v, link5_I) - fext[LINK5];
+
+ // First pass, link 'link6'
+ link6_v = ((xm->fr_link6_X_fr_link5) * link5_v);
+ link6_v(iit::rbd::AZ) += qd(JF);
+
+ iit::rbd::motionCrossProductMx<SCALAR>(link6_v, vcross);
+
+ link6_a = (xm->fr_link6_X_fr_link5) * link5_a + vcross.col(iit::rbd::AZ) * qd(JF);
+ link6_a(iit::rbd::AZ) += qdd(JF);
+
+ link6_f = link6_I * link6_a + iit::rbd::vxIv(link6_v, link6_I) - fext[LINK6];
+
+}
+
+template <typename TRAIT>
+void iit::testirb4600::dyn::tpl::InverseDynamics<TRAIT>::secondPass(JointState& jForces)
+{
+ // Link 'link6'
+ jForces(JF) = link6_f(iit::rbd::AZ);
+ link5_f += xm->fr_link6_X_fr_link5.transpose() * link6_f;
+ // Link 'link5'
+ jForces(JE) = link5_f(iit::rbd::AZ);
+ link4_f += xm->fr_link5_X_fr_link4.transpose() * link5_f;
+ // Link 'link4'
+ jForces(JD) = link4_f(iit::rbd::AZ);
+ link3_f += xm->fr_link4_X_fr_link3.transpose() * link4_f;
+ // Link 'link3'
+ jForces(JC) = link3_f(iit::rbd::AZ);
+ link2_f += xm->fr_link3_X_fr_link2.transpose() * link3_f;
+ // Link 'link2'
+ jForces(JB) = link2_f(iit::rbd::AZ);
+ link1_f += xm->fr_link2_X_fr_link1.transpose() * link2_f;
+ // Link 'link1'
+ jForces(JA) = link1_f(iit::rbd::AZ);
+}
diff --git a/ct_rbd/test/models/testIrb4600/generated/jacobians.h b/ct_rbd/test/models/testIrb4600/generated/jacobians.h
new file mode 100644
index 0000000..98ffaff
--- /dev/null
+++ b/ct_rbd/test/models/testIrb4600/generated/jacobians.h
@@ -0,0 +1,59 @@
+#ifndef TESTIRB4600_JACOBIANS_H_
+#define TESTIRB4600_JACOBIANS_H_
+
+#include <iit/rbd/TransformsBase.h>
+#include <iit/rbd/traits/DoubleTrait.h>
+#include "declarations.h"
+#include "kinematics_parameters.h"
+
+namespace iit {
+namespace testirb4600 {
+
+template<typename SCALAR, int COLS, class M>
+class JacobianT : public iit::rbd::JacobianBase<tpl::JointState<SCALAR>, COLS, M>
+{};
+
+namespace tpl{
+
+/**
+ *
+ */
+template <typename TRAIT>
+class Jacobians {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef typename TRAIT::Scalar SCALAR;
+
+ typedef JointState<SCALAR> JState;
+
+ class Type_fr_link0_J_fr_ee : public JacobianT<SCALAR, 6, Type_fr_link0_J_fr_ee>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_J_fr_ee();
+ const Type_fr_link0_J_fr_ee& update(const JState&);
+ protected:
+ };
+
+ public:
+ Jacobians();
+ void updateParameters();
+ public:
+ Type_fr_link0_J_fr_ee fr_link0_J_fr_ee;
+
+ protected:
+
+};
+
+} //namespace tpl
+
+using Jacobians = tpl::Jacobians<rbd::DoubleTrait>;
+
+#include "jacobians.impl.h"
+
+
+}
+}
+
+#endif
diff --git a/ct_rbd/test/models/testIrb4600/generated/jacobians.impl.h b/ct_rbd/test/models/testIrb4600/generated/jacobians.impl.h
new file mode 100644
index 0000000..a51b870
--- /dev/null
+++ b/ct_rbd/test/models/testIrb4600/generated/jacobians.impl.h
@@ -0,0 +1,82 @@
+
+template <typename TRAIT>
+iit::testirb4600::tpl::Jacobians<TRAIT>::Jacobians
+ ()
+ :
+ fr_link0_J_fr_ee()
+{
+ updateParameters();
+}
+
+template <typename TRAIT>
+void iit::testirb4600::tpl::Jacobians<TRAIT>::updateParameters() {
+}
+
+
+template <typename TRAIT>
+iit::testirb4600::tpl::Jacobians<TRAIT>::Type_fr_link0_J_fr_ee::Type_fr_link0_J_fr_ee()
+{
+ (*this)(0,0) = 0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,5) = 0;
+}
+
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::Jacobians<TRAIT>::Type_fr_link0_J_fr_ee& iit::testirb4600::tpl::Jacobians<TRAIT>::Type_fr_link0_J_fr_ee::update(const JState& jState) {
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+
+ sin__q_jA__ = TRAIT::sin( jState(JA));
+ sin__q_jB__ = TRAIT::sin( jState(JB));
+ sin__q_jC__ = TRAIT::sin( jState(JC));
+ sin__q_jD__ = TRAIT::sin( jState(JD));
+ sin__q_jE__ = TRAIT::sin( jState(JE));
+ cos__q_jA__ = TRAIT::cos( jState(JA));
+ cos__q_jB__ = TRAIT::cos( jState(JB));
+ cos__q_jC__ = TRAIT::cos( jState(JC));
+ cos__q_jD__ = TRAIT::cos( jState(JD));
+ cos__q_jE__ = TRAIT::cos( jState(JE));
+
+ (*this)(0,1) = - sin__q_jA__;
+ (*this)(0,2) = - sin__q_jA__;
+ (*this)(0,3) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(0,4) = ((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(0,5) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(1,1) = cos__q_jA__;
+ (*this)(1,2) = cos__q_jA__;
+ (*this)(1,3) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(1,4) = ((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(1,5) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(2,3) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(2,4) = ((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ (*this)(2,5) = ((((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(3,0) = (((((((((((( 0.443 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.443 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.443 * cos__q_jA__) * sin__q_jD__)) * sin__q_jE__) + ((((( 0.443 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__) - ((( 0.443 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__)) * cos__q_jE__)) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__)) + ((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 1.271 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * sin__q_jB__)) - ( 0.175 * sin__q_jA__));
+ (*this)(3,1) = (((((((((( 0.443 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__) - ((( 0.443 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((((- 0.443 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.443 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jE__)) + ((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__)) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 1.271 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * cos__q_jB__));
+ (*this)(3,2) = ((((((((( 0.443 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__) - ((( 0.443 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((((- 0.443 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.443 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jE__)) + ((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__)) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 1.271 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__));
+ (*this)(3,3) = ((((((( 0.443 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.443 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - (( 0.443 * sin__q_jA__) * cos__q_jD__)) * sin__q_jE__);
+ (*this)(3,4) = (((((( 0.443 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__) - ((( 0.443 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + (((((((- 0.443 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.443 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.443 * sin__q_jA__) * sin__q_jD__)) * cos__q_jE__));
+ (*this)(4,0) = ((((((((((((- 0.443 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.443 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.443 * sin__q_jA__) * sin__q_jD__)) * sin__q_jE__) + ((((( 0.443 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.443 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__)) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 1.271 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__)) + (((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * sin__q_jB__)) + ( 0.175 * cos__q_jA__));
+ (*this)(4,1) = (((((((((( 0.443 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__) - ((( 0.443 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((((- 0.443 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.443 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jE__)) + ((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__)) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 1.271 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * cos__q_jB__));
+ (*this)(4,2) = ((((((((( 0.443 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__) - ((( 0.443 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((((- 0.443 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.443 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jE__)) + ((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__)) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 1.271 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__));
+ (*this)(4,3) = ((((((( 0.443 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.443 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + (( 0.443 * cos__q_jA__) * cos__q_jD__)) * sin__q_jE__);
+ (*this)(4,4) = (((((( 0.443 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__) - ((( 0.443 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + (((( 0.443 * cos__q_jA__) * sin__q_jD__) + (((((- 0.443 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.443 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(5,1) = ((((((((( 0.443 * cos__q_jB__) * sin__q_jC__) + (( 0.443 * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((( 0.443 * sin__q_jB__) * sin__q_jC__) - (( 0.443 * cos__q_jB__) * cos__q_jC__)) * cos__q_jE__)) + ((( 1.271 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__)) + (((- 0.175 * sin__q_jB__) - ( 1.271 * cos__q_jB__)) * cos__q_jC__)) - ( 1.095 * sin__q_jB__));
+ (*this)(5,2) = (((((((( 0.443 * cos__q_jB__) * sin__q_jC__) + (( 0.443 * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((( 0.443 * sin__q_jB__) * sin__q_jC__) - (( 0.443 * cos__q_jB__) * cos__q_jC__)) * cos__q_jE__)) + ((( 1.271 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__)) + (((- 0.175 * sin__q_jB__) - ( 1.271 * cos__q_jB__)) * cos__q_jC__));
+ (*this)(5,3) = ((((( 0.443 * cos__q_jB__) * cos__q_jC__) - (( 0.443 * sin__q_jB__) * sin__q_jC__)) * sin__q_jD__) * sin__q_jE__);
+ (*this)(5,4) = ((((( 0.443 * cos__q_jB__) * sin__q_jC__) + (( 0.443 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.443 * sin__q_jB__) * sin__q_jC__) - (( 0.443 * cos__q_jB__) * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__));
+ return *this;
+}
diff --git a/ct_rbd/test/models/testIrb4600/generated/joint_data_map.h b/ct_rbd/test/models/testIrb4600/generated/joint_data_map.h
new file mode 100644
index 0000000..0ec9e02
--- /dev/null
+++ b/ct_rbd/test/models/testIrb4600/generated/joint_data_map.h
@@ -0,0 +1,106 @@
+#ifndef IIT_TESTIRB4600_JOINT_DATA_MAP_H_
+#define IIT_TESTIRB4600_JOINT_DATA_MAP_H_
+
+#include "declarations.h"
+
+namespace iit {
+namespace testirb4600 {
+
+/**
+ * A very simple container to associate a generic data item to each joint
+ */
+template<typename T> class JointDataMap {
+private:
+ T data[jointsCount];
+public:
+ JointDataMap() {};
+ JointDataMap(const T& defaultValue);
+ JointDataMap(const JointDataMap& rhs);
+ JointDataMap& operator=(const JointDataMap& rhs);
+ JointDataMap& operator=(const T& rhs);
+ T& operator[](JointIdentifiers which);
+ const T& operator[](JointIdentifiers which) const;
+private:
+ void copydata(const JointDataMap& rhs);
+ void assigndata(const T& rhs);
+};
+
+template<typename T> inline
+JointDataMap<T>::JointDataMap(const T& value) {
+ assigndata(value);
+}
+
+template<typename T> inline
+JointDataMap<T>::JointDataMap(const JointDataMap& rhs)
+{
+ copydata(rhs);
+}
+
+template<typename T> inline
+JointDataMap<T>& JointDataMap<T>::operator=(const JointDataMap& rhs)
+{
+ if(&rhs != this) {
+ copydata(rhs);
+ }
+ return *this;
+}
+
+template<typename T> inline
+JointDataMap<T>& JointDataMap<T>::operator=(const T& value)
+{
+ assigndata(value);
+ return *this;
+}
+
+template<typename T> inline
+T& JointDataMap<T>::operator[](JointIdentifiers j) {
+ return data[j];
+}
+
+template<typename T> inline
+const T& JointDataMap<T>::operator[](JointIdentifiers j) const {
+ return data[j];
+}
+
+template<typename T> inline
+void JointDataMap<T>::copydata(const JointDataMap& rhs) {
+ data[JA] = rhs[JA];
+ data[JB] = rhs[JB];
+ data[JC] = rhs[JC];
+ data[JD] = rhs[JD];
+ data[JE] = rhs[JE];
+ data[JF] = rhs[JF];
+}
+
+template<typename T> inline
+void JointDataMap<T>::assigndata(const T& value) {
+ data[JA] = value;
+ data[JB] = value;
+ data[JC] = value;
+ data[JD] = value;
+ data[JE] = value;
+ data[JF] = value;
+}
+
+template<typename T> inline
+std::ostream& operator<<(std::ostream& out, const JointDataMap<T>& map) {
+ out
+ << " jA = "
+ << map[JA]
+ << " jB = "
+ << map[JB]
+ << " jC = "
+ << map[JC]
+ << " jD = "
+ << map[JD]
+ << " jE = "
+ << map[JE]
+ << " jF = "
+ << map[JF]
+ ;
+ return out;
+}
+
+}
+}
+#endif
diff --git a/ct_rbd/test/models/testIrb4600/generated/jsim.h b/ct_rbd/test/models/testIrb4600/generated/jsim.h
new file mode 100644
index 0000000..d63560d
--- /dev/null
+++ b/ct_rbd/test/models/testIrb4600/generated/jsim.h
@@ -0,0 +1,109 @@
+#ifndef IIT_TESTIRB4600_JSIM_H_
+#define IIT_TESTIRB4600_JSIM_H_
+
+#include <iit/rbd/rbd.h>
+#include <iit/rbd/StateDependentMatrix.h>
+
+#include "declarations.h"
+#include "transforms.h"
+#include "inertia_properties.h"
+#include <iit/rbd/robcogen_commons.h>
+#include <iit/rbd/traits/DoubleTrait.h>
+
+
+namespace iit {
+namespace testirb4600 {
+namespace dyn {
+
+namespace tpl{
+
+/**
+ * The type of the Joint Space Inertia Matrix (JSIM) of the robot testirb4600.
+ */
+template <class TRAIT>
+class JSIM : public iit::rbd::StateDependentMatrix<iit::testirb4600::JointState, 6, 6, JSIM<TRAIT>>
+{
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ private:
+ typedef iit::rbd::StateDependentMatrix<iit::testirb4600::JointState, 6, 6, JSIM<TRAIT>> Base;
+ public:
+ typedef typename TRAIT::Scalar SCALAR;
+ typedef typename Base::Index Index;
+ typedef Eigen::Matrix<SCALAR,6,6> MatrixType;
+ typedef InertiaProperties<TRAIT> IProperties;
+ typedef iit::testirb4600::tpl::ForceTransforms<TRAIT> FTransforms;
+ typedef iit::rbd::tpl::InertiaMatrixDense<SCALAR> InertiaMatrix;
+
+ public:
+ JSIM(IProperties&, FTransforms&);
+ ~JSIM() {}
+
+ const JSIM& update(const iit::testirb4600::JointState&);
+
+
+ /**
+ * Computes and saves the matrix L of the L^T L factorization of this JSIM.
+ */
+ void computeL();
+ /**
+ * Computes and saves the inverse of this JSIM.
+ * This function assumes that computeL() has been called already, since it
+ * uses L to compute the inverse. The algorithm takes advantage of the branch
+ * induced sparsity of the robot, if any.
+ */
+ void computeInverse();
+ /**
+ * Returns an unmodifiable reference to the matrix L. See also computeL()
+ */
+ const MatrixType& getL() const;
+ /**
+ * Returns an unmodifiable reference to the inverse of this JSIM
+ */
+ const MatrixType& getInverse() const;
+
+ protected:
+ /**
+ * Computes and saves the inverse of the matrix L. See also computeL()
+ */
+ void computeLInverse();
+ private:
+ IProperties& linkInertias;
+ FTransforms* frcTransf;
+
+ // The composite-inertia tensor for each link
+ InertiaMatrix link1_Ic;
+ InertiaMatrix link2_Ic;
+ InertiaMatrix link3_Ic;
+ InertiaMatrix link4_Ic;
+ InertiaMatrix link5_Ic;
+ const InertiaMatrix& link6_Ic;
+ InertiaMatrix Ic_spare;
+
+ MatrixType L;
+ MatrixType Linv;
+ MatrixType inverse;
+};
+
+template <class TRAIT>
+inline const typename JSIM<TRAIT>::MatrixType& JSIM<TRAIT>::getL() const {
+ return L;
+}
+
+template <class TRAIT>
+inline const typename JSIM<TRAIT>::MatrixType& JSIM<TRAIT>::getInverse() const {
+ return inverse;
+}
+
+
+} // namespace tpl
+
+typedef tpl::JSIM<rbd::DoubleTrait> JSIM;
+
+}
+}
+}
+
+#include "jsim.impl.h"
+
+#endif
diff --git a/ct_rbd/test/models/testIrb4600/generated/jsim.impl.h b/ct_rbd/test/models/testIrb4600/generated/jsim.impl.h
new file mode 100644
index 0000000..8737671
--- /dev/null
+++ b/ct_rbd/test/models/testIrb4600/generated/jsim.impl.h
@@ -0,0 +1,273 @@
+
+
+//Implementation of default constructor
+template<typename TRAIT>
+iit::testirb4600::dyn::tpl::JSIM<TRAIT>::JSIM(IProperties& inertiaProperties, FTransforms& forceTransforms) :
+ linkInertias(inertiaProperties),
+ frcTransf( &forceTransforms ),
+ link6_Ic(linkInertias.getTensor_link6())
+{
+ //Initialize the matrix itself
+ this->setZero();
+}
+
+#define DATA tpl::JSIM<TRAIT>::operator()
+
+template <typename TRAIT>
+const typename iit::testirb4600::dyn::tpl::JSIM<TRAIT>& iit::testirb4600::dyn::tpl::JSIM<TRAIT>::update(const JointState& state) {
+ static iit::rbd::ForceVector F;
+
+ // Precomputes only once the coordinate transforms:
+ frcTransf -> fr_link5_X_fr_link6(state);
+ frcTransf -> fr_link4_X_fr_link5(state);
+ frcTransf -> fr_link3_X_fr_link4(state);
+ frcTransf -> fr_link2_X_fr_link3(state);
+ frcTransf -> fr_link1_X_fr_link2(state);
+
+ // Initializes the composite inertia tensors
+ link1_Ic = linkInertias.getTensor_link1();
+ link2_Ic = linkInertias.getTensor_link2();
+ link3_Ic = linkInertias.getTensor_link3();
+ link4_Ic = linkInertias.getTensor_link4();
+ link5_Ic = linkInertias.getTensor_link5();
+
+ // "Bottom-up" loop to update the inertia-composite property of each link, for the current configuration
+
+ // Link link6:
+ iit::rbd::transformInertia(link6_Ic, frcTransf -> fr_link5_X_fr_link6, Ic_spare);
+ link5_Ic += Ic_spare;
+
+ F = link6_Ic.col(iit::rbd::AZ);
+ DATA(JF, JF) = F(iit::rbd::AZ);
+
+ F = frcTransf -> fr_link5_X_fr_link6 * F;
+ DATA(JF, JE) = F(iit::rbd::AZ);
+ DATA(JE, JF) = DATA(JF, JE);
+ F = frcTransf -> fr_link4_X_fr_link5 * F;
+ DATA(JF, JD) = F(iit::rbd::AZ);
+ DATA(JD, JF) = DATA(JF, JD);
+ F = frcTransf -> fr_link3_X_fr_link4 * F;
+ DATA(JF, JC) = F(iit::rbd::AZ);
+ DATA(JC, JF) = DATA(JF, JC);
+ F = frcTransf -> fr_link2_X_fr_link3 * F;
+ DATA(JF, JB) = F(iit::rbd::AZ);
+ DATA(JB, JF) = DATA(JF, JB);
+ F = frcTransf -> fr_link1_X_fr_link2 * F;
+ DATA(JF, JA) = F(iit::rbd::AZ);
+ DATA(JA, JF) = DATA(JF, JA);
+
+ // Link link5:
+ iit::rbd::transformInertia(link5_Ic, frcTransf -> fr_link4_X_fr_link5, Ic_spare);
+ link4_Ic += Ic_spare;
+
+ F = link5_Ic.col(iit::rbd::AZ);
+ DATA(JE, JE) = F(iit::rbd::AZ);
+
+ F = frcTransf -> fr_link4_X_fr_link5 * F;
+ DATA(JE, JD) = F(iit::rbd::AZ);
+ DATA(JD, JE) = DATA(JE, JD);
+ F = frcTransf -> fr_link3_X_fr_link4 * F;
+ DATA(JE, JC) = F(iit::rbd::AZ);
+ DATA(JC, JE) = DATA(JE, JC);
+ F = frcTransf -> fr_link2_X_fr_link3 * F;
+ DATA(JE, JB) = F(iit::rbd::AZ);
+ DATA(JB, JE) = DATA(JE, JB);
+ F = frcTransf -> fr_link1_X_fr_link2 * F;
+ DATA(JE, JA) = F(iit::rbd::AZ);
+ DATA(JA, JE) = DATA(JE, JA);
+
+ // Link link4:
+ iit::rbd::transformInertia(link4_Ic, frcTransf -> fr_link3_X_fr_link4, Ic_spare);
+ link3_Ic += Ic_spare;
+
+ F = link4_Ic.col(iit::rbd::AZ);
+ DATA(JD, JD) = F(iit::rbd::AZ);
+
+ F = frcTransf -> fr_link3_X_fr_link4 * F;
+ DATA(JD, JC) = F(iit::rbd::AZ);
+ DATA(JC, JD) = DATA(JD, JC);
+ F = frcTransf -> fr_link2_X_fr_link3 * F;
+ DATA(JD, JB) = F(iit::rbd::AZ);
+ DATA(JB, JD) = DATA(JD, JB);
+ F = frcTransf -> fr_link1_X_fr_link2 * F;
+ DATA(JD, JA) = F(iit::rbd::AZ);
+ DATA(JA, JD) = DATA(JD, JA);
+
+ // Link link3:
+ iit::rbd::transformInertia(link3_Ic, frcTransf -> fr_link2_X_fr_link3, Ic_spare);
+ link2_Ic += Ic_spare;
+
+ F = link3_Ic.col(iit::rbd::AZ);
+ DATA(JC, JC) = F(iit::rbd::AZ);
+
+ F = frcTransf -> fr_link2_X_fr_link3 * F;
+ DATA(JC, JB) = F(iit::rbd::AZ);
+ DATA(JB, JC) = DATA(JC, JB);
+ F = frcTransf -> fr_link1_X_fr_link2 * F;
+ DATA(JC, JA) = F(iit::rbd::AZ);
+ DATA(JA, JC) = DATA(JC, JA);
+
+ // Link link2:
+ iit::rbd::transformInertia(link2_Ic, frcTransf -> fr_link1_X_fr_link2, Ic_spare);
+ link1_Ic += Ic_spare;
+
+ F = link2_Ic.col(iit::rbd::AZ);
+ DATA(JB, JB) = F(iit::rbd::AZ);
+
+ F = frcTransf -> fr_link1_X_fr_link2 * F;
+ DATA(JB, JA) = F(iit::rbd::AZ);
+ DATA(JA, JB) = DATA(JB, JA);
+
+ // Link link1:
+
+ F = link1_Ic.col(iit::rbd::AZ);
+ DATA(JA, JA) = F(iit::rbd::AZ);
+
+
+ return *this;
+}
+
+#undef DATA
+#undef F
+
+template <typename TRAIT>
+void iit::testirb4600::dyn::tpl::JSIM<TRAIT>::computeL() {
+ L = this -> template triangularView<Eigen::Lower>();
+ // Joint jF, index 5 :
+ L(5, 5) = std::sqrt(L(5, 5));
+ L(5, 4) = L(5, 4) / L(5, 5);
+ L(5, 3) = L(5, 3) / L(5, 5);
+ L(5, 2) = L(5, 2) / L(5, 5);
+ L(5, 1) = L(5, 1) / L(5, 5);
+ L(5, 0) = L(5, 0) / L(5, 5);
+ L(4, 4) = L(4, 4) - L(5, 4) * L(5, 4);
+ L(4, 3) = L(4, 3) - L(5, 4) * L(5, 3);
+ L(4, 2) = L(4, 2) - L(5, 4) * L(5, 2);
+ L(4, 1) = L(4, 1) - L(5, 4) * L(5, 1);
+ L(4, 0) = L(4, 0) - L(5, 4) * L(5, 0);
+ L(3, 3) = L(3, 3) - L(5, 3) * L(5, 3);
+ L(3, 2) = L(3, 2) - L(5, 3) * L(5, 2);
+ L(3, 1) = L(3, 1) - L(5, 3) * L(5, 1);
+ L(3, 0) = L(3, 0) - L(5, 3) * L(5, 0);
+ L(2, 2) = L(2, 2) - L(5, 2) * L(5, 2);
+ L(2, 1) = L(2, 1) - L(5, 2) * L(5, 1);
+ L(2, 0) = L(2, 0) - L(5, 2) * L(5, 0);
+ L(1, 1) = L(1, 1) - L(5, 1) * L(5, 1);
+ L(1, 0) = L(1, 0) - L(5, 1) * L(5, 0);
+ L(0, 0) = L(0, 0) - L(5, 0) * L(5, 0);
+
+ // Joint jE, index 4 :
+ L(4, 4) = std::sqrt(L(4, 4));
+ L(4, 3) = L(4, 3) / L(4, 4);
+ L(4, 2) = L(4, 2) / L(4, 4);
+ L(4, 1) = L(4, 1) / L(4, 4);
+ L(4, 0) = L(4, 0) / L(4, 4);
+ L(3, 3) = L(3, 3) - L(4, 3) * L(4, 3);
+ L(3, 2) = L(3, 2) - L(4, 3) * L(4, 2);
+ L(3, 1) = L(3, 1) - L(4, 3) * L(4, 1);
+ L(3, 0) = L(3, 0) - L(4, 3) * L(4, 0);
+ L(2, 2) = L(2, 2) - L(4, 2) * L(4, 2);
+ L(2, 1) = L(2, 1) - L(4, 2) * L(4, 1);
+ L(2, 0) = L(2, 0) - L(4, 2) * L(4, 0);
+ L(1, 1) = L(1, 1) - L(4, 1) * L(4, 1);
+ L(1, 0) = L(1, 0) - L(4, 1) * L(4, 0);
+ L(0, 0) = L(0, 0) - L(4, 0) * L(4, 0);
+
+ // Joint jD, index 3 :
+ L(3, 3) = std::sqrt(L(3, 3));
+ L(3, 2) = L(3, 2) / L(3, 3);
+ L(3, 1) = L(3, 1) / L(3, 3);
+ L(3, 0) = L(3, 0) / L(3, 3);
+ L(2, 2) = L(2, 2) - L(3, 2) * L(3, 2);
+ L(2, 1) = L(2, 1) - L(3, 2) * L(3, 1);
+ L(2, 0) = L(2, 0) - L(3, 2) * L(3, 0);
+ L(1, 1) = L(1, 1) - L(3, 1) * L(3, 1);
+ L(1, 0) = L(1, 0) - L(3, 1) * L(3, 0);
+ L(0, 0) = L(0, 0) - L(3, 0) * L(3, 0);
+
+ // Joint jC, index 2 :
+ L(2, 2) = std::sqrt(L(2, 2));
+ L(2, 1) = L(2, 1) / L(2, 2);
+ L(2, 0) = L(2, 0) / L(2, 2);
+ L(1, 1) = L(1, 1) - L(2, 1) * L(2, 1);
+ L(1, 0) = L(1, 0) - L(2, 1) * L(2, 0);
+ L(0, 0) = L(0, 0) - L(2, 0) * L(2, 0);
+
+ // Joint jB, index 1 :
+ L(1, 1) = std::sqrt(L(1, 1));
+ L(1, 0) = L(1, 0) / L(1, 1);
+ L(0, 0) = L(0, 0) - L(1, 0) * L(1, 0);
+
+ // Joint jA, index 0 :
+ L(0, 0) = std::sqrt(L(0, 0));
+
+}
+
+template <typename TRAIT>
+void iit::testirb4600::dyn::tpl::JSIM<TRAIT>::computeInverse() {
+ computeLInverse();
+
+ inverse(0, 0) = + (Linv(0, 0) * Linv(0, 0));
+ inverse(1, 1) = + (Linv(1, 0) * Linv(1, 0)) + (Linv(1, 1) * Linv(1, 1));
+ inverse(1, 0) = + (Linv(1, 0) * Linv(0, 0));
+ inverse(0, 1) = inverse(1, 0);
+ inverse(2, 2) = + (Linv(2, 0) * Linv(2, 0)) + (Linv(2, 1) * Linv(2, 1)) + (Linv(2, 2) * Linv(2, 2));
+ inverse(2, 1) = + (Linv(2, 0) * Linv(1, 0)) + (Linv(2, 1) * Linv(1, 1));
+ inverse(1, 2) = inverse(2, 1);
+ inverse(2, 0) = + (Linv(2, 0) * Linv(0, 0));
+ inverse(0, 2) = inverse(2, 0);
+ inverse(3, 3) = + (Linv(3, 0) * Linv(3, 0)) + (Linv(3, 1) * Linv(3, 1)) + (Linv(3, 2) * Linv(3, 2)) + (Linv(3, 3) * Linv(3, 3));
+ inverse(3, 2) = + (Linv(3, 0) * Linv(2, 0)) + (Linv(3, 1) * Linv(2, 1)) + (Linv(3, 2) * Linv(2, 2));
+ inverse(2, 3) = inverse(3, 2);
+ inverse(3, 1) = + (Linv(3, 0) * Linv(1, 0)) + (Linv(3, 1) * Linv(1, 1));
+ inverse(1, 3) = inverse(3, 1);
+ inverse(3, 0) = + (Linv(3, 0) * Linv(0, 0));
+ inverse(0, 3) = inverse(3, 0);
+ inverse(4, 4) = + (Linv(4, 0) * Linv(4, 0)) + (Linv(4, 1) * Linv(4, 1)) + (Linv(4, 2) * Linv(4, 2)) + (Linv(4, 3) * Linv(4, 3)) + (Linv(4, 4) * Linv(4, 4));
+ inverse(4, 3) = + (Linv(4, 0) * Linv(3, 0)) + (Linv(4, 1) * Linv(3, 1)) + (Linv(4, 2) * Linv(3, 2)) + (Linv(4, 3) * Linv(3, 3));
+ inverse(3, 4) = inverse(4, 3);
+ inverse(4, 2) = + (Linv(4, 0) * Linv(2, 0)) + (Linv(4, 1) * Linv(2, 1)) + (Linv(4, 2) * Linv(2, 2));
+ inverse(2, 4) = inverse(4, 2);
+ inverse(4, 1) = + (Linv(4, 0) * Linv(1, 0)) + (Linv(4, 1) * Linv(1, 1));
+ inverse(1, 4) = inverse(4, 1);
+ inverse(4, 0) = + (Linv(4, 0) * Linv(0, 0));
+ inverse(0, 4) = inverse(4, 0);
+ inverse(5, 5) = + (Linv(5, 0) * Linv(5, 0)) + (Linv(5, 1) * Linv(5, 1)) + (Linv(5, 2) * Linv(5, 2)) + (Linv(5, 3) * Linv(5, 3)) + (Linv(5, 4) * Linv(5, 4)) + (Linv(5, 5) * Linv(5, 5));
+ inverse(5, 4) = + (Linv(5, 0) * Linv(4, 0)) + (Linv(5, 1) * Linv(4, 1)) + (Linv(5, 2) * Linv(4, 2)) + (Linv(5, 3) * Linv(4, 3)) + (Linv(5, 4) * Linv(4, 4));
+ inverse(4, 5) = inverse(5, 4);
+ inverse(5, 3) = + (Linv(5, 0) * Linv(3, 0)) + (Linv(5, 1) * Linv(3, 1)) + (Linv(5, 2) * Linv(3, 2)) + (Linv(5, 3) * Linv(3, 3));
+ inverse(3, 5) = inverse(5, 3);
+ inverse(5, 2) = + (Linv(5, 0) * Linv(2, 0)) + (Linv(5, 1) * Linv(2, 1)) + (Linv(5, 2) * Linv(2, 2));
+ inverse(2, 5) = inverse(5, 2);
+ inverse(5, 1) = + (Linv(5, 0) * Linv(1, 0)) + (Linv(5, 1) * Linv(1, 1));
+ inverse(1, 5) = inverse(5, 1);
+ inverse(5, 0) = + (Linv(5, 0) * Linv(0, 0));
+ inverse(0, 5) = inverse(5, 0);
+}
+
+template <typename TRAIT>
+void iit::testirb4600::dyn::tpl::JSIM<TRAIT>::computeLInverse() {
+ //assumes L has been computed already
+ Linv(0, 0) = 1 / L(0, 0);
+ Linv(1, 1) = 1 / L(1, 1);
+ Linv(2, 2) = 1 / L(2, 2);
+ Linv(3, 3) = 1 / L(3, 3);
+ Linv(4, 4) = 1 / L(4, 4);
+ Linv(5, 5) = 1 / L(5, 5);
+ Linv(1, 0) = - Linv(0, 0) * ((Linv(1, 1) * L(1, 0)) + 0);
+ Linv(2, 1) = - Linv(1, 1) * ((Linv(2, 2) * L(2, 1)) + 0);
+ Linv(2, 0) = - Linv(0, 0) * ((Linv(2, 1) * L(1, 0)) + (Linv(2, 2) * L(2, 0)) + 0);
+ Linv(3, 2) = - Linv(2, 2) * ((Linv(3, 3) * L(3, 2)) + 0);
+ Linv(3, 1) = - Linv(1, 1) * ((Linv(3, 2) * L(2, 1)) + (Linv(3, 3) * L(3, 1)) + 0);
+ Linv(3, 0) = - Linv(0, 0) * ((Linv(3, 1) * L(1, 0)) + (Linv(3, 2) * L(2, 0)) + (Linv(3, 3) * L(3, 0)) + 0);
+ Linv(4, 3) = - Linv(3, 3) * ((Linv(4, 4) * L(4, 3)) + 0);
+ Linv(4, 2) = - Linv(2, 2) * ((Linv(4, 3) * L(3, 2)) + (Linv(4, 4) * L(4, 2)) + 0);
+ Linv(4, 1) = - Linv(1, 1) * ((Linv(4, 2) * L(2, 1)) + (Linv(4, 3) * L(3, 1)) + (Linv(4, 4) * L(4, 1)) + 0);
+ Linv(4, 0) = - Linv(0, 0) * ((Linv(4, 1) * L(1, 0)) + (Linv(4, 2) * L(2, 0)) + (Linv(4, 3) * L(3, 0)) + (Linv(4, 4) * L(4, 0)) + 0);
+ Linv(5, 4) = - Linv(4, 4) * ((Linv(5, 5) * L(5, 4)) + 0);
+ Linv(5, 3) = - Linv(3, 3) * ((Linv(5, 4) * L(4, 3)) + (Linv(5, 5) * L(5, 3)) + 0);
+ Linv(5, 2) = - Linv(2, 2) * ((Linv(5, 3) * L(3, 2)) + (Linv(5, 4) * L(4, 2)) + (Linv(5, 5) * L(5, 2)) + 0);
+ Linv(5, 1) = - Linv(1, 1) * ((Linv(5, 2) * L(2, 1)) + (Linv(5, 3) * L(3, 1)) + (Linv(5, 4) * L(4, 1)) + (Linv(5, 5) * L(5, 1)) + 0);
+ Linv(5, 0) = - Linv(0, 0) * ((Linv(5, 1) * L(1, 0)) + (Linv(5, 2) * L(2, 0)) + (Linv(5, 3) * L(3, 0)) + (Linv(5, 4) * L(4, 0)) + (Linv(5, 5) * L(5, 0)) + 0);
+}
+
diff --git a/ct_rbd/test/models/testIrb4600/generated/kinematics_parameters.h b/ct_rbd/test/models/testIrb4600/generated/kinematics_parameters.h
new file mode 100644
index 0000000..1b8dc6c
--- /dev/null
+++ b/ct_rbd/test/models/testIrb4600/generated/kinematics_parameters.h
@@ -0,0 +1,11 @@
+#ifndef _TESTIRB4600_PARAMETERS_GETTERS_
+#define _TESTIRB4600_PARAMETERS_GETTERS_
+
+namespace iit {
+namespace testirb4600 {
+
+
+
+}
+}
+#endif
diff --git a/ct_rbd/test/models/testIrb4600/generated/link_data_map.h b/ct_rbd/test/models/testIrb4600/generated/link_data_map.h
new file mode 100644
index 0000000..b32aae8
--- /dev/null
+++ b/ct_rbd/test/models/testIrb4600/generated/link_data_map.h
@@ -0,0 +1,112 @@
+#ifndef IIT_TESTIRB4600_LINK_DATA_MAP_H_
+#define IIT_TESTIRB4600_LINK_DATA_MAP_H_
+
+#include "declarations.h"
+
+namespace iit {
+namespace testirb4600 {
+
+/**
+ * A very simple container to associate a generic data item to each link
+ */
+template<typename T> class LinkDataMap {
+private:
+ T data[linksCount];
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ LinkDataMap() {};
+ LinkDataMap(const T& defaultValue);
+ LinkDataMap(const LinkDataMap& rhs);
+ LinkDataMap& operator=(const LinkDataMap& rhs);
+ LinkDataMap& operator=(const T& rhs);
+ T& operator[](LinkIdentifiers which);
+ const T& operator[](LinkIdentifiers which) const;
+private:
+ void copydata(const LinkDataMap& rhs);
+ void assigndata(const T& commonValue);
+};
+
+template<typename T> inline
+LinkDataMap<T>::LinkDataMap(const T& value) {
+ assigndata(value);
+}
+
+template<typename T> inline
+LinkDataMap<T>::LinkDataMap(const LinkDataMap& rhs)
+{
+ copydata(rhs);
+}
+
+template<typename T> inline
+LinkDataMap<T>& LinkDataMap<T>::operator=(const LinkDataMap& rhs)
+{
+ if(&rhs != this) {
+ copydata(rhs);
+ }
+ return *this;
+}
+
+template<typename T> inline
+LinkDataMap<T>& LinkDataMap<T>::operator=(const T& value)
+{
+ assigndata(value);
+ return *this;
+}
+
+template<typename T> inline
+T& LinkDataMap<T>::operator[](LinkIdentifiers l) {
+ return data[l];
+}
+
+template<typename T> inline
+const T& LinkDataMap<T>::operator[](LinkIdentifiers l) const {
+ return data[l];
+}
+
+template<typename T> inline
+void LinkDataMap<T>::copydata(const LinkDataMap& rhs) {
+ data[LINK0] = rhs[LINK0];
+ data[LINK1] = rhs[LINK1];
+ data[LINK2] = rhs[LINK2];
+ data[LINK3] = rhs[LINK3];
+ data[LINK4] = rhs[LINK4];
+ data[LINK5] = rhs[LINK5];
+ data[LINK6] = rhs[LINK6];
+}
+
+template<typename T> inline
+void LinkDataMap<T>::assigndata(const T& value) {
+ data[LINK0] = value;
+ data[LINK1] = value;
+ data[LINK2] = value;
+ data[LINK3] = value;
+ data[LINK4] = value;
+ data[LINK5] = value;
+ data[LINK6] = value;
+}
+
+template<typename T> inline
+std::ostream& operator<<(std::ostream& out, const LinkDataMap<T>& map) {
+ out
+ << " link0 = "
+ << map[LINK0]
+ << " link1 = "
+ << map[LINK1]
+ << " link2 = "
+ << map[LINK2]
+ << " link3 = "
+ << map[LINK3]
+ << " link4 = "
+ << map[LINK4]
+ << " link5 = "
+ << map[LINK5]
+ << " link6 = "
+ << map[LINK6]
+ ;
+ return out;
+}
+
+}
+}
+#endif
diff --git a/ct_rbd/test/models/testIrb4600/generated/miscellaneous.cpp b/ct_rbd/test/models/testIrb4600/generated/miscellaneous.cpp
new file mode 100644
index 0000000..05af2f6
--- /dev/null
+++ b/ct_rbd/test/models/testIrb4600/generated/miscellaneous.cpp
@@ -0,0 +1,58 @@
+#include <iit/rbd/utils.h>
+#include "miscellaneous.h"
+
+using namespace iit::testirb4600;
+using namespace iit::testirb4600::dyn;
+
+iit::rbd::Vector3d iit::testirb4600::getWholeBodyCOM(
+ const InertiaProperties& inertiaProps,
+ const HomogeneousTransforms& ht)
+{
+ iit::rbd::Vector3d tmpSum(iit::rbd::Vector3d::Zero());
+
+
+ HomogeneousTransforms::MatrixType tmpX(HomogeneousTransforms::MatrixType::Identity());
+ tmpX = tmpX * ht.fr_link0_X_fr_link1;
+ tmpSum += inertiaProps.getMass_link1() *
+ ( iit::rbd::Utils::transform(tmpX, inertiaProps.getCOM_link1()));
+
+ tmpX = tmpX * ht.fr_link1_X_fr_link2;
+ tmpSum += inertiaProps.getMass_link2() *
+ ( iit::rbd::Utils::transform(tmpX, inertiaProps.getCOM_link2()));
+
+ tmpX = tmpX * ht.fr_link2_X_fr_link3;
+ tmpSum += inertiaProps.getMass_link3() *
+ ( iit::rbd::Utils::transform(tmpX, inertiaProps.getCOM_link3()));
+
+ tmpX = tmpX * ht.fr_link3_X_fr_link4;
+ tmpSum += inertiaProps.getMass_link4() *
+ ( iit::rbd::Utils::transform(tmpX, inertiaProps.getCOM_link4()));
+
+ tmpX = tmpX * ht.fr_link4_X_fr_link5;
+ tmpSum += inertiaProps.getMass_link5() *
+ ( iit::rbd::Utils::transform(tmpX, inertiaProps.getCOM_link5()));
+
+ tmpX = tmpX * ht.fr_link5_X_fr_link6;
+ tmpSum += inertiaProps.getMass_link6() *
+ ( iit::rbd::Utils::transform(tmpX, inertiaProps.getCOM_link6()));
+
+
+ return tmpSum / inertiaProps.getTotalMass();
+}
+
+iit::rbd::Vector3d iit::testirb4600::getWholeBodyCOM(
+ const InertiaProperties& inertiaProps,
+ const JointState& q,
+ HomogeneousTransforms& ht)
+{
+ // First updates the coordinate transforms that will be used by the routine
+ ht.fr_link0_X_fr_link1(q);
+ ht.fr_link1_X_fr_link2(q);
+ ht.fr_link2_X_fr_link3(q);
+ ht.fr_link3_X_fr_link4(q);
+ ht.fr_link4_X_fr_link5(q);
+ ht.fr_link5_X_fr_link6(q);
+
+ // The actual calculus
+ return getWholeBodyCOM(inertiaProps, ht);
+}
diff --git a/ct_rbd/test/models/testIrb4600/generated/miscellaneous.h b/ct_rbd/test/models/testIrb4600/generated/miscellaneous.h
new file mode 100644
index 0000000..5aad17d
--- /dev/null
+++ b/ct_rbd/test/models/testIrb4600/generated/miscellaneous.h
@@ -0,0 +1,45 @@
+#ifndef IIT_ROBCOGEN__TESTIRB4600_MISCELLANEOUS_H_
+#define IIT_ROBCOGEN__TESTIRB4600_MISCELLANEOUS_H_
+
+#include "inertia_properties.h"
+#include "transforms.h"
+
+namespace iit {
+namespace testirb4600 {
+
+/** \name Center of mass calculation
+ * Computes the Center Of Mass (COM) position of the whole robot, in
+ * base coordinates.
+ *
+ * Common parameters are the inertia properties of the robot and the set
+ * of homogeneous coordinate transforms. If a joint status variable is
+ * also passed, then the transforms are updated accordingly; otherwise,
+ * they are not modified before being used.
+ */
+///@{
+/**
+ * \param inertia the inertia properties of the links of the robot
+ * \param transforms the homogeneous coordinate transforms of the robot
+ * \return the position of the Center Of Mass of the whole robot, expressed
+ * in base coordinates
+ */
+iit::rbd::Vector3d getWholeBodyCOM(
+ const dyn::InertiaProperties& inertia,
+ const HomogeneousTransforms& transforms);
+/**
+ * \param inertia the inertia properties of the links of the robot
+ * \param q the joint status vector describing the configuration of the robot
+ * \param transforms the homogeneous coordinate transforms of the robot
+ * \return the position of the Center Of Mass of the whole robot, expressed
+ * in base coordinates
+ */
+iit::rbd::Vector3d getWholeBodyCOM(
+ const dyn::InertiaProperties& inertia,
+ const JointState& q,
+ HomogeneousTransforms& transforms);
+///@}
+
+}
+}
+
+#endif
diff --git a/ct_rbd/test/models/testIrb4600/generated/traits.h b/ct_rbd/test/models/testIrb4600/generated/traits.h
new file mode 100644
index 0000000..429e5ee
--- /dev/null
+++ b/ct_rbd/test/models/testIrb4600/generated/traits.h
@@ -0,0 +1,66 @@
+#ifndef IIT_ROBOGEN__TESTIRB4600_TRAITS_H_
+#define IIT_ROBOGEN__TESTIRB4600_TRAITS_H_
+
+#include "declarations.h"
+#include "transforms.h"
+#include "inverse_dynamics.h"
+#include "forward_dynamics.h"
+#include "jsim.h"
+#include "inertia_properties.h"
+#include "jacobians.h"
+#include <iit/rbd/traits/TraitSelector.h>
+
+
+namespace iit {
+namespace testirb4600 {
+
+namespace tpl{
+
+template <typename SCALAR>
+struct Traits {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef SCALAR S;
+
+ typedef typename iit::rbd::tpl::TraitSelector<SCALAR>::Trait Trait;
+
+ typedef typename testirb4600::tpl::JointState<SCALAR> JointState;
+
+ typedef typename testirb4600::JointIdentifiers JointID;
+ typedef typename testirb4600::LinkIdentifiers LinkID;
+
+ typedef typename testirb4600::tpl::HomogeneousTransforms<Trait> HomogeneousTransforms;
+ typedef typename testirb4600::tpl::MotionTransforms<Trait> MotionTransforms;
+ typedef typename testirb4600::tpl::ForceTransforms<Trait> ForceTransforms;
+ typedef typename testirb4600::tpl::Jacobians<Trait> Jacobians;
+
+ typedef typename testirb4600::dyn::tpl::InertiaProperties<Trait> InertiaProperties;
+ typedef typename testirb4600::dyn::tpl::ForwardDynamics<Trait> FwdDynEngine;
+ typedef typename testirb4600::dyn::tpl::InverseDynamics<Trait> InvDynEngine;
+ typedef typename testirb4600::dyn::tpl::JSIM<Trait> JSIM;
+
+ static const int joints_count = testirb4600::jointsCount;
+ static const int links_count = testirb4600::linksCount;
+ static const bool floating_base = false;
+
+ static inline const JointID* orderedJointIDs();
+ static inline const LinkID* orderedLinkIDs();
+};
+
+template <typename SCALAR>
+inline const typename Traits<SCALAR>::JointID* Traits<SCALAR>::orderedJointIDs() {
+ return testirb4600::orderedJointIDs;
+}
+template <typename SCALAR>
+inline const typename Traits<SCALAR>::LinkID* Traits<SCALAR>::orderedLinkIDs() {
+ return testirb4600::orderedLinkIDs;
+}
+
+} // namespace tpl
+
+typedef tpl::Traits<double> Traits;
+
+}
+}
+
+#endif
diff --git a/ct_rbd/test/models/testIrb4600/generated/transforms.h b/ct_rbd/test/models/testIrb4600/generated/transforms.h
new file mode 100644
index 0000000..1357b32
--- /dev/null
+++ b/ct_rbd/test/models/testIrb4600/generated/transforms.h
@@ -0,0 +1,1431 @@
+#ifndef TESTIRB4600_TRANSFORMS_H_
+#define TESTIRB4600_TRANSFORMS_H_
+
+#include <Eigen/Dense>
+#include <iit/rbd/TransformsBase.h>
+#include "declarations.h"
+#include <iit/rbd/traits/DoubleTrait.h>
+#include "kinematics_parameters.h"
+
+namespace iit {
+namespace testirb4600 {
+
+template<typename SCALAR, class M>
+class TransformMotion : public iit::rbd::SpatialTransformBase<tpl::JointState<SCALAR>, M> {
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+template<typename SCALAR, class M>
+class TransformForce : public iit::rbd::SpatialTransformBase<tpl::JointState<SCALAR>, M> {
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+template<typename SCALAR, class M>
+class TransformHomogeneous : public iit::rbd::HomogeneousTransformBase<tpl::JointState<SCALAR>, M> {
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+namespace tpl {
+
+
+/**
+ * The class for the 6-by-6 coordinates transformation matrices for
+ * spatial motion vectors.
+ */
+template <typename TRAIT>
+class MotionTransforms {
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef typename TRAIT::Scalar SCALAR;
+
+ typedef JointState<SCALAR> JState;
+ class Dummy {};
+ typedef typename TransformMotion<SCALAR, Dummy>::MatrixType MatrixType;
+public:
+ class Type_fr_link0_X_fr_link1 : public TransformMotion<SCALAR, Type_fr_link0_X_fr_link1>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_link1();
+ const Type_fr_link0_X_fr_link1& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_link2 : public TransformMotion<SCALAR, Type_fr_link0_X_fr_link2>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_link2();
+ const Type_fr_link0_X_fr_link2& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_link3 : public TransformMotion<SCALAR, Type_fr_link0_X_fr_link3>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_link3();
+ const Type_fr_link0_X_fr_link3& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_link4 : public TransformMotion<SCALAR, Type_fr_link0_X_fr_link4>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_link4();
+ const Type_fr_link0_X_fr_link4& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_link5 : public TransformMotion<SCALAR, Type_fr_link0_X_fr_link5>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_link5();
+ const Type_fr_link0_X_fr_link5& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_link6 : public TransformMotion<SCALAR, Type_fr_link0_X_fr_link6>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_link6();
+ const Type_fr_link0_X_fr_link6& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_ee : public TransformMotion<SCALAR, Type_fr_link0_X_fr_ee>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_ee();
+ const Type_fr_link0_X_fr_ee& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_com0 : public TransformMotion<SCALAR, Type_fr_link0_X_com0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_com0();
+ const Type_fr_link0_X_com0& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_com1 : public TransformMotion<SCALAR, Type_fr_link0_X_com1>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_com1();
+ const Type_fr_link0_X_com1& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_com2 : public TransformMotion<SCALAR, Type_fr_link0_X_com2>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_com2();
+ const Type_fr_link0_X_com2& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_com3 : public TransformMotion<SCALAR, Type_fr_link0_X_com3>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_com3();
+ const Type_fr_link0_X_com3& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_com4 : public TransformMotion<SCALAR, Type_fr_link0_X_com4>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_com4();
+ const Type_fr_link0_X_com4& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_com5 : public TransformMotion<SCALAR, Type_fr_link0_X_com5>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_com5();
+ const Type_fr_link0_X_com5& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_com6 : public TransformMotion<SCALAR, Type_fr_link0_X_com6>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_com6();
+ const Type_fr_link0_X_com6& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link1_X_fr_link0 : public TransformMotion<SCALAR, Type_fr_link1_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link1_X_fr_link0();
+ const Type_fr_link1_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link2_X_fr_link0 : public TransformMotion<SCALAR, Type_fr_link2_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link2_X_fr_link0();
+ const Type_fr_link2_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link3_X_fr_link0 : public TransformMotion<SCALAR, Type_fr_link3_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link3_X_fr_link0();
+ const Type_fr_link3_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link4_X_fr_link0 : public TransformMotion<SCALAR, Type_fr_link4_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link4_X_fr_link0();
+ const Type_fr_link4_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link5_X_fr_link0 : public TransformMotion<SCALAR, Type_fr_link5_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link5_X_fr_link0();
+ const Type_fr_link5_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link6_X_fr_link0 : public TransformMotion<SCALAR, Type_fr_link6_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link6_X_fr_link0();
+ const Type_fr_link6_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_ee_X_fr_link0 : public TransformMotion<SCALAR, Type_fr_ee_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_ee_X_fr_link0();
+ const Type_fr_ee_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_com0_X_fr_link0 : public TransformMotion<SCALAR, Type_com0_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_com0_X_fr_link0();
+ const Type_com0_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_com1_X_fr_link0 : public TransformMotion<SCALAR, Type_com1_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_com1_X_fr_link0();
+ const Type_com1_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_com2_X_fr_link0 : public TransformMotion<SCALAR, Type_com2_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_com2_X_fr_link0();
+ const Type_com2_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_com3_X_fr_link0 : public TransformMotion<SCALAR, Type_com3_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_com3_X_fr_link0();
+ const Type_com3_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_com4_X_fr_link0 : public TransformMotion<SCALAR, Type_com4_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_com4_X_fr_link0();
+ const Type_com4_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_com5_X_fr_link0 : public TransformMotion<SCALAR, Type_com5_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_com5_X_fr_link0();
+ const Type_com5_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_com6_X_fr_link0 : public TransformMotion<SCALAR, Type_com6_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_com6_X_fr_link0();
+ const Type_com6_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_jA : public TransformMotion<SCALAR, Type_fr_link0_X_fr_jA>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_jA();
+ const Type_fr_link0_X_fr_jA& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_jB : public TransformMotion<SCALAR, Type_fr_link0_X_fr_jB>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_jB();
+ const Type_fr_link0_X_fr_jB& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_jC : public TransformMotion<SCALAR, Type_fr_link0_X_fr_jC>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_jC();
+ const Type_fr_link0_X_fr_jC& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_jD : public TransformMotion<SCALAR, Type_fr_link0_X_fr_jD>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_jD();
+ const Type_fr_link0_X_fr_jD& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_jE : public TransformMotion<SCALAR, Type_fr_link0_X_fr_jE>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_jE();
+ const Type_fr_link0_X_fr_jE& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_jF : public TransformMotion<SCALAR, Type_fr_link0_X_fr_jF>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_jF();
+ const Type_fr_link0_X_fr_jF& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link2_X_fr_link1 : public TransformMotion<SCALAR, Type_fr_link2_X_fr_link1>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link2_X_fr_link1();
+ const Type_fr_link2_X_fr_link1& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link1_X_fr_link2 : public TransformMotion<SCALAR, Type_fr_link1_X_fr_link2>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link1_X_fr_link2();
+ const Type_fr_link1_X_fr_link2& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link3_X_fr_link2 : public TransformMotion<SCALAR, Type_fr_link3_X_fr_link2>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link3_X_fr_link2();
+ const Type_fr_link3_X_fr_link2& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link2_X_fr_link3 : public TransformMotion<SCALAR, Type_fr_link2_X_fr_link3>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link2_X_fr_link3();
+ const Type_fr_link2_X_fr_link3& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link4_X_fr_link3 : public TransformMotion<SCALAR, Type_fr_link4_X_fr_link3>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link4_X_fr_link3();
+ const Type_fr_link4_X_fr_link3& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link3_X_fr_link4 : public TransformMotion<SCALAR, Type_fr_link3_X_fr_link4>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link3_X_fr_link4();
+ const Type_fr_link3_X_fr_link4& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link5_X_fr_link4 : public TransformMotion<SCALAR, Type_fr_link5_X_fr_link4>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link5_X_fr_link4();
+ const Type_fr_link5_X_fr_link4& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link4_X_fr_link5 : public TransformMotion<SCALAR, Type_fr_link4_X_fr_link5>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link4_X_fr_link5();
+ const Type_fr_link4_X_fr_link5& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link6_X_fr_link5 : public TransformMotion<SCALAR, Type_fr_link6_X_fr_link5>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link6_X_fr_link5();
+ const Type_fr_link6_X_fr_link5& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link5_X_fr_link6 : public TransformMotion<SCALAR, Type_fr_link5_X_fr_link6>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link5_X_fr_link6();
+ const Type_fr_link5_X_fr_link6& update(const JState&);
+ protected:
+ };
+
+public:
+ MotionTransforms();
+ void updateParameters();
+ Type_fr_link0_X_fr_link1 fr_link0_X_fr_link1;
+ Type_fr_link0_X_fr_link2 fr_link0_X_fr_link2;
+ Type_fr_link0_X_fr_link3 fr_link0_X_fr_link3;
+ Type_fr_link0_X_fr_link4 fr_link0_X_fr_link4;
+ Type_fr_link0_X_fr_link5 fr_link0_X_fr_link5;
+ Type_fr_link0_X_fr_link6 fr_link0_X_fr_link6;
+ Type_fr_link0_X_fr_ee fr_link0_X_fr_ee;
+ Type_fr_link0_X_com0 fr_link0_X_com0;
+ Type_fr_link0_X_com1 fr_link0_X_com1;
+ Type_fr_link0_X_com2 fr_link0_X_com2;
+ Type_fr_link0_X_com3 fr_link0_X_com3;
+ Type_fr_link0_X_com4 fr_link0_X_com4;
+ Type_fr_link0_X_com5 fr_link0_X_com5;
+ Type_fr_link0_X_com6 fr_link0_X_com6;
+ Type_fr_link1_X_fr_link0 fr_link1_X_fr_link0;
+ Type_fr_link2_X_fr_link0 fr_link2_X_fr_link0;
+ Type_fr_link3_X_fr_link0 fr_link3_X_fr_link0;
+ Type_fr_link4_X_fr_link0 fr_link4_X_fr_link0;
+ Type_fr_link5_X_fr_link0 fr_link5_X_fr_link0;
+ Type_fr_link6_X_fr_link0 fr_link6_X_fr_link0;
+ Type_fr_ee_X_fr_link0 fr_ee_X_fr_link0;
+ Type_com0_X_fr_link0 com0_X_fr_link0;
+ Type_com1_X_fr_link0 com1_X_fr_link0;
+ Type_com2_X_fr_link0 com2_X_fr_link0;
+ Type_com3_X_fr_link0 com3_X_fr_link0;
+ Type_com4_X_fr_link0 com4_X_fr_link0;
+ Type_com5_X_fr_link0 com5_X_fr_link0;
+ Type_com6_X_fr_link0 com6_X_fr_link0;
+ Type_fr_link0_X_fr_jA fr_link0_X_fr_jA;
+ Type_fr_link0_X_fr_jB fr_link0_X_fr_jB;
+ Type_fr_link0_X_fr_jC fr_link0_X_fr_jC;
+ Type_fr_link0_X_fr_jD fr_link0_X_fr_jD;
+ Type_fr_link0_X_fr_jE fr_link0_X_fr_jE;
+ Type_fr_link0_X_fr_jF fr_link0_X_fr_jF;
+ Type_fr_link2_X_fr_link1 fr_link2_X_fr_link1;
+ Type_fr_link1_X_fr_link2 fr_link1_X_fr_link2;
+ Type_fr_link3_X_fr_link2 fr_link3_X_fr_link2;
+ Type_fr_link2_X_fr_link3 fr_link2_X_fr_link3;
+ Type_fr_link4_X_fr_link3 fr_link4_X_fr_link3;
+ Type_fr_link3_X_fr_link4 fr_link3_X_fr_link4;
+ Type_fr_link5_X_fr_link4 fr_link5_X_fr_link4;
+ Type_fr_link4_X_fr_link5 fr_link4_X_fr_link5;
+ Type_fr_link6_X_fr_link5 fr_link6_X_fr_link5;
+ Type_fr_link5_X_fr_link6 fr_link5_X_fr_link6;
+
+protected:
+
+}; //class 'MotionTransforms'
+
+/**
+ * The class for the 6-by-6 coordinates transformation matrices for
+ * spatial force vectors.
+ */
+template <typename TRAIT>
+class ForceTransforms {
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef typename TRAIT::Scalar SCALAR;
+
+ typedef JointState<SCALAR> JState;
+ class Dummy {};
+ typedef typename TransformForce<SCALAR, Dummy>::MatrixType MatrixType;
+public:
+ class Type_fr_link0_X_fr_link1 : public TransformForce<SCALAR, Type_fr_link0_X_fr_link1>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_link1();
+ const Type_fr_link0_X_fr_link1& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_link2 : public TransformForce<SCALAR, Type_fr_link0_X_fr_link2>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_link2();
+ const Type_fr_link0_X_fr_link2& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_link3 : public TransformForce<SCALAR, Type_fr_link0_X_fr_link3>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_link3();
+ const Type_fr_link0_X_fr_link3& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_link4 : public TransformForce<SCALAR, Type_fr_link0_X_fr_link4>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_link4();
+ const Type_fr_link0_X_fr_link4& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_link5 : public TransformForce<SCALAR, Type_fr_link0_X_fr_link5>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_link5();
+ const Type_fr_link0_X_fr_link5& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_link6 : public TransformForce<SCALAR, Type_fr_link0_X_fr_link6>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_link6();
+ const Type_fr_link0_X_fr_link6& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_ee : public TransformForce<SCALAR, Type_fr_link0_X_fr_ee>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_ee();
+ const Type_fr_link0_X_fr_ee& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_com0 : public TransformForce<SCALAR, Type_fr_link0_X_com0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_com0();
+ const Type_fr_link0_X_com0& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_com1 : public TransformForce<SCALAR, Type_fr_link0_X_com1>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_com1();
+ const Type_fr_link0_X_com1& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_com2 : public TransformForce<SCALAR, Type_fr_link0_X_com2>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_com2();
+ const Type_fr_link0_X_com2& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_com3 : public TransformForce<SCALAR, Type_fr_link0_X_com3>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_com3();
+ const Type_fr_link0_X_com3& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_com4 : public TransformForce<SCALAR, Type_fr_link0_X_com4>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_com4();
+ const Type_fr_link0_X_com4& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_com5 : public TransformForce<SCALAR, Type_fr_link0_X_com5>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_com5();
+ const Type_fr_link0_X_com5& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_com6 : public TransformForce<SCALAR, Type_fr_link0_X_com6>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_com6();
+ const Type_fr_link0_X_com6& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link1_X_fr_link0 : public TransformForce<SCALAR, Type_fr_link1_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link1_X_fr_link0();
+ const Type_fr_link1_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link2_X_fr_link0 : public TransformForce<SCALAR, Type_fr_link2_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link2_X_fr_link0();
+ const Type_fr_link2_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link3_X_fr_link0 : public TransformForce<SCALAR, Type_fr_link3_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link3_X_fr_link0();
+ const Type_fr_link3_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link4_X_fr_link0 : public TransformForce<SCALAR, Type_fr_link4_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link4_X_fr_link0();
+ const Type_fr_link4_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link5_X_fr_link0 : public TransformForce<SCALAR, Type_fr_link5_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link5_X_fr_link0();
+ const Type_fr_link5_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link6_X_fr_link0 : public TransformForce<SCALAR, Type_fr_link6_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link6_X_fr_link0();
+ const Type_fr_link6_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_ee_X_fr_link0 : public TransformForce<SCALAR, Type_fr_ee_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_ee_X_fr_link0();
+ const Type_fr_ee_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_com0_X_fr_link0 : public TransformForce<SCALAR, Type_com0_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_com0_X_fr_link0();
+ const Type_com0_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_com1_X_fr_link0 : public TransformForce<SCALAR, Type_com1_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_com1_X_fr_link0();
+ const Type_com1_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_com2_X_fr_link0 : public TransformForce<SCALAR, Type_com2_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_com2_X_fr_link0();
+ const Type_com2_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_com3_X_fr_link0 : public TransformForce<SCALAR, Type_com3_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_com3_X_fr_link0();
+ const Type_com3_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_com4_X_fr_link0 : public TransformForce<SCALAR, Type_com4_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_com4_X_fr_link0();
+ const Type_com4_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_com5_X_fr_link0 : public TransformForce<SCALAR, Type_com5_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_com5_X_fr_link0();
+ const Type_com5_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_com6_X_fr_link0 : public TransformForce<SCALAR, Type_com6_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_com6_X_fr_link0();
+ const Type_com6_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_jA : public TransformForce<SCALAR, Type_fr_link0_X_fr_jA>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_jA();
+ const Type_fr_link0_X_fr_jA& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_jB : public TransformForce<SCALAR, Type_fr_link0_X_fr_jB>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_jB();
+ const Type_fr_link0_X_fr_jB& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_jC : public TransformForce<SCALAR, Type_fr_link0_X_fr_jC>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_jC();
+ const Type_fr_link0_X_fr_jC& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_jD : public TransformForce<SCALAR, Type_fr_link0_X_fr_jD>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_jD();
+ const Type_fr_link0_X_fr_jD& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_jE : public TransformForce<SCALAR, Type_fr_link0_X_fr_jE>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_jE();
+ const Type_fr_link0_X_fr_jE& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_jF : public TransformForce<SCALAR, Type_fr_link0_X_fr_jF>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_jF();
+ const Type_fr_link0_X_fr_jF& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link2_X_fr_link1 : public TransformForce<SCALAR, Type_fr_link2_X_fr_link1>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link2_X_fr_link1();
+ const Type_fr_link2_X_fr_link1& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link1_X_fr_link2 : public TransformForce<SCALAR, Type_fr_link1_X_fr_link2>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link1_X_fr_link2();
+ const Type_fr_link1_X_fr_link2& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link3_X_fr_link2 : public TransformForce<SCALAR, Type_fr_link3_X_fr_link2>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link3_X_fr_link2();
+ const Type_fr_link3_X_fr_link2& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link2_X_fr_link3 : public TransformForce<SCALAR, Type_fr_link2_X_fr_link3>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link2_X_fr_link3();
+ const Type_fr_link2_X_fr_link3& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link4_X_fr_link3 : public TransformForce<SCALAR, Type_fr_link4_X_fr_link3>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link4_X_fr_link3();
+ const Type_fr_link4_X_fr_link3& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link3_X_fr_link4 : public TransformForce<SCALAR, Type_fr_link3_X_fr_link4>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link3_X_fr_link4();
+ const Type_fr_link3_X_fr_link4& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link5_X_fr_link4 : public TransformForce<SCALAR, Type_fr_link5_X_fr_link4>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link5_X_fr_link4();
+ const Type_fr_link5_X_fr_link4& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link4_X_fr_link5 : public TransformForce<SCALAR, Type_fr_link4_X_fr_link5>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link4_X_fr_link5();
+ const Type_fr_link4_X_fr_link5& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link6_X_fr_link5 : public TransformForce<SCALAR, Type_fr_link6_X_fr_link5>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link6_X_fr_link5();
+ const Type_fr_link6_X_fr_link5& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link5_X_fr_link6 : public TransformForce<SCALAR, Type_fr_link5_X_fr_link6>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link5_X_fr_link6();
+ const Type_fr_link5_X_fr_link6& update(const JState&);
+ protected:
+ };
+
+public:
+ ForceTransforms();
+ void updateParameters();
+ Type_fr_link0_X_fr_link1 fr_link0_X_fr_link1;
+ Type_fr_link0_X_fr_link2 fr_link0_X_fr_link2;
+ Type_fr_link0_X_fr_link3 fr_link0_X_fr_link3;
+ Type_fr_link0_X_fr_link4 fr_link0_X_fr_link4;
+ Type_fr_link0_X_fr_link5 fr_link0_X_fr_link5;
+ Type_fr_link0_X_fr_link6 fr_link0_X_fr_link6;
+ Type_fr_link0_X_fr_ee fr_link0_X_fr_ee;
+ Type_fr_link0_X_com0 fr_link0_X_com0;
+ Type_fr_link0_X_com1 fr_link0_X_com1;
+ Type_fr_link0_X_com2 fr_link0_X_com2;
+ Type_fr_link0_X_com3 fr_link0_X_com3;
+ Type_fr_link0_X_com4 fr_link0_X_com4;
+ Type_fr_link0_X_com5 fr_link0_X_com5;
+ Type_fr_link0_X_com6 fr_link0_X_com6;
+ Type_fr_link1_X_fr_link0 fr_link1_X_fr_link0;
+ Type_fr_link2_X_fr_link0 fr_link2_X_fr_link0;
+ Type_fr_link3_X_fr_link0 fr_link3_X_fr_link0;
+ Type_fr_link4_X_fr_link0 fr_link4_X_fr_link0;
+ Type_fr_link5_X_fr_link0 fr_link5_X_fr_link0;
+ Type_fr_link6_X_fr_link0 fr_link6_X_fr_link0;
+ Type_fr_ee_X_fr_link0 fr_ee_X_fr_link0;
+ Type_com0_X_fr_link0 com0_X_fr_link0;
+ Type_com1_X_fr_link0 com1_X_fr_link0;
+ Type_com2_X_fr_link0 com2_X_fr_link0;
+ Type_com3_X_fr_link0 com3_X_fr_link0;
+ Type_com4_X_fr_link0 com4_X_fr_link0;
+ Type_com5_X_fr_link0 com5_X_fr_link0;
+ Type_com6_X_fr_link0 com6_X_fr_link0;
+ Type_fr_link0_X_fr_jA fr_link0_X_fr_jA;
+ Type_fr_link0_X_fr_jB fr_link0_X_fr_jB;
+ Type_fr_link0_X_fr_jC fr_link0_X_fr_jC;
+ Type_fr_link0_X_fr_jD fr_link0_X_fr_jD;
+ Type_fr_link0_X_fr_jE fr_link0_X_fr_jE;
+ Type_fr_link0_X_fr_jF fr_link0_X_fr_jF;
+ Type_fr_link2_X_fr_link1 fr_link2_X_fr_link1;
+ Type_fr_link1_X_fr_link2 fr_link1_X_fr_link2;
+ Type_fr_link3_X_fr_link2 fr_link3_X_fr_link2;
+ Type_fr_link2_X_fr_link3 fr_link2_X_fr_link3;
+ Type_fr_link4_X_fr_link3 fr_link4_X_fr_link3;
+ Type_fr_link3_X_fr_link4 fr_link3_X_fr_link4;
+ Type_fr_link5_X_fr_link4 fr_link5_X_fr_link4;
+ Type_fr_link4_X_fr_link5 fr_link4_X_fr_link5;
+ Type_fr_link6_X_fr_link5 fr_link6_X_fr_link5;
+ Type_fr_link5_X_fr_link6 fr_link5_X_fr_link6;
+
+protected:
+
+}; //class 'ForceTransforms'
+
+/**
+ * The class with the homogeneous (4x4) coordinates transformation
+ * matrices.
+ */
+template <typename TRAIT>
+class HomogeneousTransforms {
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef typename TRAIT::Scalar SCALAR;
+
+ typedef JointState<SCALAR> JState;
+ class Dummy {};
+ typedef typename TransformHomogeneous<SCALAR, Dummy>::MatrixType MatrixType;
+public:
+ class Type_fr_link0_X_fr_link1 : public TransformHomogeneous<SCALAR, Type_fr_link0_X_fr_link1>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_link1();
+ const Type_fr_link0_X_fr_link1& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_link2 : public TransformHomogeneous<SCALAR, Type_fr_link0_X_fr_link2>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_link2();
+ const Type_fr_link0_X_fr_link2& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_link3 : public TransformHomogeneous<SCALAR, Type_fr_link0_X_fr_link3>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_link3();
+ const Type_fr_link0_X_fr_link3& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_link4 : public TransformHomogeneous<SCALAR, Type_fr_link0_X_fr_link4>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_link4();
+ const Type_fr_link0_X_fr_link4& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_link5 : public TransformHomogeneous<SCALAR, Type_fr_link0_X_fr_link5>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_link5();
+ const Type_fr_link0_X_fr_link5& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_link6 : public TransformHomogeneous<SCALAR, Type_fr_link0_X_fr_link6>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_link6();
+ const Type_fr_link0_X_fr_link6& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_ee : public TransformHomogeneous<SCALAR, Type_fr_link0_X_fr_ee>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_ee();
+ const Type_fr_link0_X_fr_ee& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_com0 : public TransformHomogeneous<SCALAR, Type_fr_link0_X_com0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_com0();
+ const Type_fr_link0_X_com0& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_com1 : public TransformHomogeneous<SCALAR, Type_fr_link0_X_com1>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_com1();
+ const Type_fr_link0_X_com1& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_com2 : public TransformHomogeneous<SCALAR, Type_fr_link0_X_com2>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_com2();
+ const Type_fr_link0_X_com2& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_com3 : public TransformHomogeneous<SCALAR, Type_fr_link0_X_com3>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_com3();
+ const Type_fr_link0_X_com3& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_com4 : public TransformHomogeneous<SCALAR, Type_fr_link0_X_com4>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_com4();
+ const Type_fr_link0_X_com4& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_com5 : public TransformHomogeneous<SCALAR, Type_fr_link0_X_com5>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_com5();
+ const Type_fr_link0_X_com5& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_com6 : public TransformHomogeneous<SCALAR, Type_fr_link0_X_com6>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_com6();
+ const Type_fr_link0_X_com6& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link1_X_fr_link0 : public TransformHomogeneous<SCALAR, Type_fr_link1_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link1_X_fr_link0();
+ const Type_fr_link1_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link2_X_fr_link0 : public TransformHomogeneous<SCALAR, Type_fr_link2_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link2_X_fr_link0();
+ const Type_fr_link2_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link3_X_fr_link0 : public TransformHomogeneous<SCALAR, Type_fr_link3_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link3_X_fr_link0();
+ const Type_fr_link3_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link4_X_fr_link0 : public TransformHomogeneous<SCALAR, Type_fr_link4_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link4_X_fr_link0();
+ const Type_fr_link4_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link5_X_fr_link0 : public TransformHomogeneous<SCALAR, Type_fr_link5_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link5_X_fr_link0();
+ const Type_fr_link5_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link6_X_fr_link0 : public TransformHomogeneous<SCALAR, Type_fr_link6_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link6_X_fr_link0();
+ const Type_fr_link6_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_ee_X_fr_link0 : public TransformHomogeneous<SCALAR, Type_fr_ee_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_ee_X_fr_link0();
+ const Type_fr_ee_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_com0_X_fr_link0 : public TransformHomogeneous<SCALAR, Type_com0_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_com0_X_fr_link0();
+ const Type_com0_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_com1_X_fr_link0 : public TransformHomogeneous<SCALAR, Type_com1_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_com1_X_fr_link0();
+ const Type_com1_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_com2_X_fr_link0 : public TransformHomogeneous<SCALAR, Type_com2_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_com2_X_fr_link0();
+ const Type_com2_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_com3_X_fr_link0 : public TransformHomogeneous<SCALAR, Type_com3_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_com3_X_fr_link0();
+ const Type_com3_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_com4_X_fr_link0 : public TransformHomogeneous<SCALAR, Type_com4_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_com4_X_fr_link0();
+ const Type_com4_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_com5_X_fr_link0 : public TransformHomogeneous<SCALAR, Type_com5_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_com5_X_fr_link0();
+ const Type_com5_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_com6_X_fr_link0 : public TransformHomogeneous<SCALAR, Type_com6_X_fr_link0>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_com6_X_fr_link0();
+ const Type_com6_X_fr_link0& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_jA : public TransformHomogeneous<SCALAR, Type_fr_link0_X_fr_jA>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_jA();
+ const Type_fr_link0_X_fr_jA& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_jB : public TransformHomogeneous<SCALAR, Type_fr_link0_X_fr_jB>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_jB();
+ const Type_fr_link0_X_fr_jB& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_jC : public TransformHomogeneous<SCALAR, Type_fr_link0_X_fr_jC>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_jC();
+ const Type_fr_link0_X_fr_jC& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_jD : public TransformHomogeneous<SCALAR, Type_fr_link0_X_fr_jD>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_jD();
+ const Type_fr_link0_X_fr_jD& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_jE : public TransformHomogeneous<SCALAR, Type_fr_link0_X_fr_jE>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_jE();
+ const Type_fr_link0_X_fr_jE& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link0_X_fr_jF : public TransformHomogeneous<SCALAR, Type_fr_link0_X_fr_jF>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link0_X_fr_jF();
+ const Type_fr_link0_X_fr_jF& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link2_X_fr_link1 : public TransformHomogeneous<SCALAR, Type_fr_link2_X_fr_link1>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link2_X_fr_link1();
+ const Type_fr_link2_X_fr_link1& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link1_X_fr_link2 : public TransformHomogeneous<SCALAR, Type_fr_link1_X_fr_link2>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link1_X_fr_link2();
+ const Type_fr_link1_X_fr_link2& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link3_X_fr_link2 : public TransformHomogeneous<SCALAR, Type_fr_link3_X_fr_link2>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link3_X_fr_link2();
+ const Type_fr_link3_X_fr_link2& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link2_X_fr_link3 : public TransformHomogeneous<SCALAR, Type_fr_link2_X_fr_link3>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link2_X_fr_link3();
+ const Type_fr_link2_X_fr_link3& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link4_X_fr_link3 : public TransformHomogeneous<SCALAR, Type_fr_link4_X_fr_link3>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link4_X_fr_link3();
+ const Type_fr_link4_X_fr_link3& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link3_X_fr_link4 : public TransformHomogeneous<SCALAR, Type_fr_link3_X_fr_link4>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link3_X_fr_link4();
+ const Type_fr_link3_X_fr_link4& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link5_X_fr_link4 : public TransformHomogeneous<SCALAR, Type_fr_link5_X_fr_link4>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link5_X_fr_link4();
+ const Type_fr_link5_X_fr_link4& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link4_X_fr_link5 : public TransformHomogeneous<SCALAR, Type_fr_link4_X_fr_link5>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link4_X_fr_link5();
+ const Type_fr_link4_X_fr_link5& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link6_X_fr_link5 : public TransformHomogeneous<SCALAR, Type_fr_link6_X_fr_link5>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link6_X_fr_link5();
+ const Type_fr_link6_X_fr_link5& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_link5_X_fr_link6 : public TransformHomogeneous<SCALAR, Type_fr_link5_X_fr_link6>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_link5_X_fr_link6();
+ const Type_fr_link5_X_fr_link6& update(const JState&);
+ protected:
+ };
+
+public:
+ HomogeneousTransforms();
+ void updateParameters();
+ Type_fr_link0_X_fr_link1 fr_link0_X_fr_link1;
+ Type_fr_link0_X_fr_link2 fr_link0_X_fr_link2;
+ Type_fr_link0_X_fr_link3 fr_link0_X_fr_link3;
+ Type_fr_link0_X_fr_link4 fr_link0_X_fr_link4;
+ Type_fr_link0_X_fr_link5 fr_link0_X_fr_link5;
+ Type_fr_link0_X_fr_link6 fr_link0_X_fr_link6;
+ Type_fr_link0_X_fr_ee fr_link0_X_fr_ee;
+ Type_fr_link0_X_com0 fr_link0_X_com0;
+ Type_fr_link0_X_com1 fr_link0_X_com1;
+ Type_fr_link0_X_com2 fr_link0_X_com2;
+ Type_fr_link0_X_com3 fr_link0_X_com3;
+ Type_fr_link0_X_com4 fr_link0_X_com4;
+ Type_fr_link0_X_com5 fr_link0_X_com5;
+ Type_fr_link0_X_com6 fr_link0_X_com6;
+ Type_fr_link1_X_fr_link0 fr_link1_X_fr_link0;
+ Type_fr_link2_X_fr_link0 fr_link2_X_fr_link0;
+ Type_fr_link3_X_fr_link0 fr_link3_X_fr_link0;
+ Type_fr_link4_X_fr_link0 fr_link4_X_fr_link0;
+ Type_fr_link5_X_fr_link0 fr_link5_X_fr_link0;
+ Type_fr_link6_X_fr_link0 fr_link6_X_fr_link0;
+ Type_fr_ee_X_fr_link0 fr_ee_X_fr_link0;
+ Type_com0_X_fr_link0 com0_X_fr_link0;
+ Type_com1_X_fr_link0 com1_X_fr_link0;
+ Type_com2_X_fr_link0 com2_X_fr_link0;
+ Type_com3_X_fr_link0 com3_X_fr_link0;
+ Type_com4_X_fr_link0 com4_X_fr_link0;
+ Type_com5_X_fr_link0 com5_X_fr_link0;
+ Type_com6_X_fr_link0 com6_X_fr_link0;
+ Type_fr_link0_X_fr_jA fr_link0_X_fr_jA;
+ Type_fr_link0_X_fr_jB fr_link0_X_fr_jB;
+ Type_fr_link0_X_fr_jC fr_link0_X_fr_jC;
+ Type_fr_link0_X_fr_jD fr_link0_X_fr_jD;
+ Type_fr_link0_X_fr_jE fr_link0_X_fr_jE;
+ Type_fr_link0_X_fr_jF fr_link0_X_fr_jF;
+ Type_fr_link2_X_fr_link1 fr_link2_X_fr_link1;
+ Type_fr_link1_X_fr_link2 fr_link1_X_fr_link2;
+ Type_fr_link3_X_fr_link2 fr_link3_X_fr_link2;
+ Type_fr_link2_X_fr_link3 fr_link2_X_fr_link3;
+ Type_fr_link4_X_fr_link3 fr_link4_X_fr_link3;
+ Type_fr_link3_X_fr_link4 fr_link3_X_fr_link4;
+ Type_fr_link5_X_fr_link4 fr_link5_X_fr_link4;
+ Type_fr_link4_X_fr_link5 fr_link4_X_fr_link5;
+ Type_fr_link6_X_fr_link5 fr_link6_X_fr_link5;
+ Type_fr_link5_X_fr_link6 fr_link5_X_fr_link6;
+
+protected:
+
+}; //class 'HomogeneousTransforms'
+
+} // namespace tpl
+
+using MotionTransforms = tpl::MotionTransforms<rbd::DoubleTrait>;
+using ForceTransforms = tpl::ForceTransforms<rbd::DoubleTrait>;
+using HomogeneousTransforms = tpl::HomogeneousTransforms<rbd::DoubleTrait>;
+
+}
+}
+
+#include "transforms.impl.h"
+
+#endif
diff --git a/ct_rbd/test/models/testIrb4600/generated/transforms.impl.h b/ct_rbd/test/models/testIrb4600/generated/transforms.impl.h
new file mode 100644
index 0000000..98454d9
--- /dev/null
+++ b/ct_rbd/test/models/testIrb4600/generated/transforms.impl.h
@@ -0,0 +1,6796 @@
+
+// Constructors
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::MotionTransforms
+ ()
+ :
+ fr_link0_X_fr_link1(),
+ fr_link0_X_fr_link2(),
+ fr_link0_X_fr_link3(),
+ fr_link0_X_fr_link4(),
+ fr_link0_X_fr_link5(),
+ fr_link0_X_fr_link6(),
+ fr_link0_X_fr_ee(),
+ fr_link0_X_com0(),
+ fr_link0_X_com1(),
+ fr_link0_X_com2(),
+ fr_link0_X_com3(),
+ fr_link0_X_com4(),
+ fr_link0_X_com5(),
+ fr_link0_X_com6(),
+ fr_link1_X_fr_link0(),
+ fr_link2_X_fr_link0(),
+ fr_link3_X_fr_link0(),
+ fr_link4_X_fr_link0(),
+ fr_link5_X_fr_link0(),
+ fr_link6_X_fr_link0(),
+ fr_ee_X_fr_link0(),
+ com0_X_fr_link0(),
+ com1_X_fr_link0(),
+ com2_X_fr_link0(),
+ com3_X_fr_link0(),
+ com4_X_fr_link0(),
+ com5_X_fr_link0(),
+ com6_X_fr_link0(),
+ fr_link0_X_fr_jA(),
+ fr_link0_X_fr_jB(),
+ fr_link0_X_fr_jC(),
+ fr_link0_X_fr_jD(),
+ fr_link0_X_fr_jE(),
+ fr_link0_X_fr_jF(),
+ fr_link2_X_fr_link1(),
+ fr_link1_X_fr_link2(),
+ fr_link3_X_fr_link2(),
+ fr_link2_X_fr_link3(),
+ fr_link4_X_fr_link3(),
+ fr_link3_X_fr_link4(),
+ fr_link5_X_fr_link4(),
+ fr_link4_X_fr_link5(),
+ fr_link6_X_fr_link5(),
+ fr_link5_X_fr_link6()
+{
+ updateParameters();
+}
+template <typename TRAIT>
+void iit::testirb4600::tpl::MotionTransforms<TRAIT>::updateParameters() {
+}
+
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::ForceTransforms
+ ()
+ :
+ fr_link0_X_fr_link1(),
+ fr_link0_X_fr_link2(),
+ fr_link0_X_fr_link3(),
+ fr_link0_X_fr_link4(),
+ fr_link0_X_fr_link5(),
+ fr_link0_X_fr_link6(),
+ fr_link0_X_fr_ee(),
+ fr_link0_X_com0(),
+ fr_link0_X_com1(),
+ fr_link0_X_com2(),
+ fr_link0_X_com3(),
+ fr_link0_X_com4(),
+ fr_link0_X_com5(),
+ fr_link0_X_com6(),
+ fr_link1_X_fr_link0(),
+ fr_link2_X_fr_link0(),
+ fr_link3_X_fr_link0(),
+ fr_link4_X_fr_link0(),
+ fr_link5_X_fr_link0(),
+ fr_link6_X_fr_link0(),
+ fr_ee_X_fr_link0(),
+ com0_X_fr_link0(),
+ com1_X_fr_link0(),
+ com2_X_fr_link0(),
+ com3_X_fr_link0(),
+ com4_X_fr_link0(),
+ com5_X_fr_link0(),
+ com6_X_fr_link0(),
+ fr_link0_X_fr_jA(),
+ fr_link0_X_fr_jB(),
+ fr_link0_X_fr_jC(),
+ fr_link0_X_fr_jD(),
+ fr_link0_X_fr_jE(),
+ fr_link0_X_fr_jF(),
+ fr_link2_X_fr_link1(),
+ fr_link1_X_fr_link2(),
+ fr_link3_X_fr_link2(),
+ fr_link2_X_fr_link3(),
+ fr_link4_X_fr_link3(),
+ fr_link3_X_fr_link4(),
+ fr_link5_X_fr_link4(),
+ fr_link4_X_fr_link5(),
+ fr_link6_X_fr_link5(),
+ fr_link5_X_fr_link6()
+{
+ updateParameters();
+}
+template <typename TRAIT>
+void iit::testirb4600::tpl::ForceTransforms<TRAIT>::updateParameters() {
+}
+
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::HomogeneousTransforms
+ ()
+ :
+ fr_link0_X_fr_link1(),
+ fr_link0_X_fr_link2(),
+ fr_link0_X_fr_link3(),
+ fr_link0_X_fr_link4(),
+ fr_link0_X_fr_link5(),
+ fr_link0_X_fr_link6(),
+ fr_link0_X_fr_ee(),
+ fr_link0_X_com0(),
+ fr_link0_X_com1(),
+ fr_link0_X_com2(),
+ fr_link0_X_com3(),
+ fr_link0_X_com4(),
+ fr_link0_X_com5(),
+ fr_link0_X_com6(),
+ fr_link1_X_fr_link0(),
+ fr_link2_X_fr_link0(),
+ fr_link3_X_fr_link0(),
+ fr_link4_X_fr_link0(),
+ fr_link5_X_fr_link0(),
+ fr_link6_X_fr_link0(),
+ fr_ee_X_fr_link0(),
+ com0_X_fr_link0(),
+ com1_X_fr_link0(),
+ com2_X_fr_link0(),
+ com3_X_fr_link0(),
+ com4_X_fr_link0(),
+ com5_X_fr_link0(),
+ com6_X_fr_link0(),
+ fr_link0_X_fr_jA(),
+ fr_link0_X_fr_jB(),
+ fr_link0_X_fr_jC(),
+ fr_link0_X_fr_jD(),
+ fr_link0_X_fr_jE(),
+ fr_link0_X_fr_jF(),
+ fr_link2_X_fr_link1(),
+ fr_link1_X_fr_link2(),
+ fr_link3_X_fr_link2(),
+ fr_link2_X_fr_link3(),
+ fr_link4_X_fr_link3(),
+ fr_link3_X_fr_link4(),
+ fr_link5_X_fr_link4(),
+ fr_link4_X_fr_link5(),
+ fr_link6_X_fr_link5(),
+ fr_link5_X_fr_link6()
+{
+ updateParameters();
+}
+template <typename TRAIT>
+void iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::updateParameters() {
+}
+
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_link1::Type_fr_link0_X_fr_link1()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_link1& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_link1::update(const JState& q) {
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+
+ (*this)(0,0) = cos__q_jA__;
+ (*this)(0,1) = - sin__q_jA__;
+ (*this)(1,0) = sin__q_jA__;
+ (*this)(1,1) = cos__q_jA__;
+ (*this)(3,3) = cos__q_jA__;
+ (*this)(3,4) = - sin__q_jA__;
+ (*this)(4,3) = sin__q_jA__;
+ (*this)(4,4) = cos__q_jA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_link2::Type_fr_link0_X_fr_link2()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0.175;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_link2& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_link2::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+
+ (*this)(0,0) = ( cos__q_jA__ * sin__q_jB__);
+ (*this)(0,1) = ( cos__q_jA__ * cos__q_jB__);
+ (*this)(0,2) = - sin__q_jA__;
+ (*this)(1,0) = ( sin__q_jA__ * sin__q_jB__);
+ (*this)(1,1) = ( sin__q_jA__ * cos__q_jB__);
+ (*this)(1,2) = cos__q_jA__;
+ (*this)(2,0) = cos__q_jB__;
+ (*this)(2,1) = - sin__q_jB__;
+ (*this)(3,0) = ((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__));
+ (*this)(3,1) = (((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__));
+ (*this)(3,2) = (- 0.495 * cos__q_jA__);
+ (*this)(3,3) = ( cos__q_jA__ * sin__q_jB__);
+ (*this)(3,4) = ( cos__q_jA__ * cos__q_jB__);
+ (*this)(3,5) = - sin__q_jA__;
+ (*this)(4,0) = ((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__));
+ (*this)(4,1) = ((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__));
+ (*this)(4,2) = (- 0.495 * sin__q_jA__);
+ (*this)(4,3) = ( sin__q_jA__ * sin__q_jB__);
+ (*this)(4,4) = ( sin__q_jA__ * cos__q_jB__);
+ (*this)(4,5) = cos__q_jA__;
+ (*this)(5,3) = cos__q_jB__;
+ (*this)(5,4) = - sin__q_jB__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_link3::Type_fr_link0_X_fr_link3()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_link3& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_link3::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+
+ (*this)(0,0) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(0,1) = (((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(0,2) = - sin__q_jA__;
+ (*this)(1,0) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(1,1) = (((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(1,2) = cos__q_jA__;
+ (*this)(2,0) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(2,1) = (( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__));
+ (*this)(3,0) = ((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__));
+ (*this)(3,1) = (((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__));
+ (*this)(3,2) = (((- 1.095 * cos__q_jA__) * cos__q_jB__) - ( 0.495 * cos__q_jA__));
+ (*this)(3,3) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(3,4) = (((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(3,5) = - sin__q_jA__;
+ (*this)(4,0) = ((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__));
+ (*this)(4,1) = ((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__));
+ (*this)(4,2) = (((- 1.095 * sin__q_jA__) * cos__q_jB__) - ( 0.495 * sin__q_jA__));
+ (*this)(4,3) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(4,4) = (((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(4,5) = cos__q_jA__;
+ (*this)(5,2) = (( 1.095 * sin__q_jB__) + 0.175);
+ (*this)(5,3) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(5,4) = (( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__));
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_link4::Type_fr_link0_X_fr_link4()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_link4& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_link4::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+
+ (*this)(0,0) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( sin__q_jA__ * cos__q_jD__));
+ (*this)(0,1) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__));
+ (*this)(0,2) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(1,0) = (((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( cos__q_jA__ * cos__q_jD__));
+ (*this)(1,1) = (( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__));
+ (*this)(1,2) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(2,0) = ((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * sin__q_jD__);
+ (*this)(2,1) = ((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__);
+ (*this)(2,2) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(3,0) = ((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 0.332 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.332 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.332 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * cos__q_jB__)) + ( 0.495 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(3,1) = ((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.332 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.332 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 0.332 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(3,2) = (((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__));
+ (*this)(3,3) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( sin__q_jA__ * cos__q_jD__));
+ (*this)(3,4) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__));
+ (*this)(3,5) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(4,0) = (((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 0.332 * cos__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.332 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.332 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * cos__q_jB__)) + ( 0.495 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(4,1) = ((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.332 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.332 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 0.332 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(4,2) = (((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__));
+ (*this)(4,3) = (((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( cos__q_jA__ * cos__q_jD__));
+ (*this)(4,4) = (( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__));
+ (*this)(4,5) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(5,0) = ((((((( 0.332 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + (((- 0.175 * sin__q_jB__) - ( 0.332 * cos__q_jB__)) * cos__q_jC__)) - ( 1.095 * sin__q_jB__)) - 0.175) * cos__q_jD__);
+ (*this)(5,1) = ((((((( 0.175 * cos__q_jB__) - ( 0.332 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 0.332 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__);
+ (*this)(5,3) = ((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * sin__q_jD__);
+ (*this)(5,4) = ((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__);
+ (*this)(5,5) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_link5::Type_fr_link0_X_fr_link5()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_link5& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_link5::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+
+ (*this)(0,0) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(0,1) = ((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__));
+ (*this)(0,2) = ((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(1,0) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(1,1) = ((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(1,2) = ((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(2,0) = ((((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(2,1) = (((( cos__q_jB__ * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__));
+ (*this)(2,2) = ((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ (*this)(3,0) = ((((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * cos__q_jE__));
+ (*this)(3,1) = (((((((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * cos__q_jC__)) + ( 0.175 * sin__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(3,2) = (((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(3,3) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(3,4) = ((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__));
+ (*this)(3,5) = (((((( 1.0 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(4,0) = ((((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * cos__q_jE__));
+ (*this)(4,1) = (((((((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * cos__q_jC__)) - ( 0.175 * cos__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(4,2) = ((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(4,3) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(4,4) = ((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(4,5) = (((((( 1.0 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(5,0) = (((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * sin__q_jE__);
+ (*this)(5,1) = (((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * cos__q_jE__);
+ (*this)(5,2) = ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__);
+ (*this)(5,3) = ((((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(5,4) = (((( cos__q_jB__ * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__));
+ (*this)(5,5) = (((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_link6::Type_fr_link0_X_fr_link6()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_link6& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_link6::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jF__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+ SCALAR cos__q_jF__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jF__ = TRAIT::sin( q(JF));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+ cos__q_jF__ = TRAIT::cos( q(JF));
+
+ (*this)(0,0) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,1) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(0,2) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(1,0) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(1,1) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,2) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(2,0) = ((((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + ((((( cos__q_jB__ * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(2,1) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(2,2) = ((((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(3,0) = (((((((((( 0.243 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.243 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.243 * sin__q_jA__) * sin__q_jD__) + ((((( 0.243 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jF__) + ((((((((((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * cos__q_jC__)) + ( 0.175 * sin__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.243 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.243 * sin__q_jA__) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(3,1) = (((((((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * sin__q_jE__) + ((((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 1.271 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * cos__q_jB__)) + ( 0.495 * cos__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + (((((- 0.243 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.243 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.243 * sin__q_jA__) * cos__q_jD__)) * sin__q_jF__) + ((((((((( 0.243 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.243 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.243 * sin__q_jA__) * sin__q_jD__) + ((((( 0.243 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(3,2) = ((((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * cos__q_jE__));
+ (*this)(3,3) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(3,4) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(3,5) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(4,0) = (((((((((( 0.243 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.243 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.243 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.243 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jF__) + ((((((((((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * cos__q_jC__)) - ( 0.175 * cos__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.243 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.243 * cos__q_jA__) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,1) = (((((((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * sin__q_jE__) + ((((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 1.271 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * cos__q_jB__)) + ( 0.495 * sin__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + (((((- 0.243 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.243 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.243 * cos__q_jA__) * cos__q_jD__)) * sin__q_jF__) + ((((((((( 0.243 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.243 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.243 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.243 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,2) = ((((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * cos__q_jE__));
+ (*this)(4,3) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(4,4) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,5) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(5,0) = ((((((((- 0.243 * cos__q_jB__) * sin__q_jC__) - (( 0.243 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.243 * cos__q_jB__) * cos__q_jC__) - (( 0.243 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__)) * sin__q_jF__) + (((((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.243 * cos__q_jB__) * cos__q_jC__) - (( 0.243 * sin__q_jB__) * sin__q_jC__)) * sin__q_jD__)) * cos__q_jF__));
+ (*this)(5,1) = ((((((((((( 1.271 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + (((- 0.175 * sin__q_jB__) - ( 1.271 * cos__q_jB__)) * cos__q_jC__)) - ( 1.095 * sin__q_jB__)) - 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.243 * sin__q_jB__) * sin__q_jC__) - (( 0.243 * cos__q_jB__) * cos__q_jC__)) * sin__q_jD__)) * sin__q_jF__) + (((((((- 0.243 * cos__q_jB__) * sin__q_jC__) - (( 0.243 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.243 * cos__q_jB__) * cos__q_jC__) - (( 0.243 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(5,2) = (((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * sin__q_jE__);
+ (*this)(5,3) = ((((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + ((((( cos__q_jB__ * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(5,4) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(5,5) = ((((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_ee::Type_fr_link0_X_fr_ee()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_ee& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_ee::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jF__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+ SCALAR cos__q_jF__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jF__ = TRAIT::sin( q(JF));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+ cos__q_jF__ = TRAIT::cos( q(JF));
+
+ (*this)(0,0) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,1) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(0,2) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(1,0) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(1,1) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,2) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(2,0) = ((((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + ((((( cos__q_jB__ * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(2,1) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(2,2) = ((((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(3,0) = (((((((((( 0.443 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.443 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.443 * sin__q_jA__) * sin__q_jD__) + ((((( 0.443 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.443 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jF__) + ((((((((((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * cos__q_jC__)) + ( 0.175 * sin__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.443 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.443 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.443 * sin__q_jA__) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(3,1) = (((((((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * sin__q_jE__) + ((((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 1.271 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * cos__q_jB__)) + ( 0.495 * cos__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + (((((- 0.443 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.443 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.443 * sin__q_jA__) * cos__q_jD__)) * sin__q_jF__) + ((((((((( 0.443 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.443 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.443 * sin__q_jA__) * sin__q_jD__) + ((((( 0.443 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.443 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(3,2) = ((((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * cos__q_jE__));
+ (*this)(3,3) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(3,4) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(3,5) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(4,0) = (((((((((( 0.443 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.443 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.443 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.443 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.443 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jF__) + ((((((((((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * cos__q_jC__)) - ( 0.175 * cos__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.443 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.443 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.443 * cos__q_jA__) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,1) = (((((((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * sin__q_jE__) + ((((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 1.271 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * cos__q_jB__)) + ( 0.495 * sin__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + (((((- 0.443 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.443 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.443 * cos__q_jA__) * cos__q_jD__)) * sin__q_jF__) + ((((((((( 0.443 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.443 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.443 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.443 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.443 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,2) = ((((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * cos__q_jE__));
+ (*this)(4,3) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(4,4) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,5) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(5,0) = ((((((((- 0.443 * cos__q_jB__) * sin__q_jC__) - (( 0.443 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.443 * cos__q_jB__) * cos__q_jC__) - (( 0.443 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__)) * sin__q_jF__) + (((((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.443 * cos__q_jB__) * cos__q_jC__) - (( 0.443 * sin__q_jB__) * sin__q_jC__)) * sin__q_jD__)) * cos__q_jF__));
+ (*this)(5,1) = ((((((((((( 1.271 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + (((- 0.175 * sin__q_jB__) - ( 1.271 * cos__q_jB__)) * cos__q_jC__)) - ( 1.095 * sin__q_jB__)) - 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.443 * sin__q_jB__) * sin__q_jC__) - (( 0.443 * cos__q_jB__) * cos__q_jC__)) * sin__q_jD__)) * sin__q_jF__) + (((((((- 0.443 * cos__q_jB__) * sin__q_jC__) - (( 0.443 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.443 * cos__q_jB__) * cos__q_jC__) - (( 0.443 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(5,2) = (((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * sin__q_jE__);
+ (*this)(5,3) = ((((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + ((((( cos__q_jB__ * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(5,4) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(5,5) = ((((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_com0::Type_fr_link0_X_com0()
+{
+ (*this)(0,0) = 1;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 1;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = - 0.075;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0.075;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = 1;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_com0& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_com0::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_com1::Type_fr_link0_X_com1()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1.0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0.09;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_com1& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_com1::update(const JState& q) {
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+
+ (*this)(0,0) = cos__q_jA__;
+ (*this)(0,1) = - sin__q_jA__;
+ (*this)(1,0) = sin__q_jA__;
+ (*this)(1,1) = cos__q_jA__;
+ (*this)(3,0) = (- 0.25 * sin__q_jA__);
+ (*this)(3,1) = (- 0.25 * cos__q_jA__);
+ (*this)(3,2) = ( 0.09 * sin__q_jA__);
+ (*this)(3,3) = cos__q_jA__;
+ (*this)(3,4) = - sin__q_jA__;
+ (*this)(4,0) = ( 0.25 * cos__q_jA__);
+ (*this)(4,1) = (- 0.25 * sin__q_jA__);
+ (*this)(4,2) = (- 0.09 * cos__q_jA__);
+ (*this)(4,3) = sin__q_jA__;
+ (*this)(4,4) = cos__q_jA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_com2::Type_fr_link0_X_com2()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_com2& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_com2::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+
+ (*this)(0,0) = ( cos__q_jA__ * sin__q_jB__);
+ (*this)(0,1) = ( cos__q_jA__ * cos__q_jB__);
+ (*this)(0,2) = - sin__q_jA__;
+ (*this)(1,0) = ( sin__q_jA__ * sin__q_jB__);
+ (*this)(1,1) = ( sin__q_jA__ * cos__q_jB__);
+ (*this)(1,2) = cos__q_jA__;
+ (*this)(2,0) = cos__q_jB__;
+ (*this)(2,1) = - sin__q_jB__;
+ (*this)(3,0) = ((((- 0.495 * sin__q_jA__) * sin__q_jB__) + ((( 0.175 * sin__q_jA__) - ( 0.17 * cos__q_jA__)) * cos__q_jB__)) + ( 0.04 * sin__q_jA__));
+ (*this)(3,1) = ((((( 0.17 * cos__q_jA__) - ( 0.175 * sin__q_jA__)) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 0.47 * sin__q_jA__));
+ (*this)(3,2) = (((( 0.04 * cos__q_jA__) * sin__q_jB__) - (( 0.47 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__));
+ (*this)(3,3) = ( cos__q_jA__ * sin__q_jB__);
+ (*this)(3,4) = ( cos__q_jA__ * cos__q_jB__);
+ (*this)(3,5) = - sin__q_jA__;
+ (*this)(4,0) = (((( 0.495 * cos__q_jA__) * sin__q_jB__) + (((- 0.17 * sin__q_jA__) - ( 0.175 * cos__q_jA__)) * cos__q_jB__)) - ( 0.04 * cos__q_jA__));
+ (*this)(4,1) = ((((( 0.17 * sin__q_jA__) + ( 0.175 * cos__q_jA__)) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 0.47 * cos__q_jA__));
+ (*this)(4,2) = (((( 0.04 * sin__q_jA__) * sin__q_jB__) - (( 0.47 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__));
+ (*this)(4,3) = ( sin__q_jA__ * sin__q_jB__);
+ (*this)(4,4) = ( sin__q_jA__ * cos__q_jB__);
+ (*this)(4,5) = cos__q_jA__;
+ (*this)(5,0) = ( 0.17 * sin__q_jB__);
+ (*this)(5,1) = ( 0.17 * cos__q_jB__);
+ (*this)(5,2) = ((( 0.47 * sin__q_jB__) + ( 0.04 * cos__q_jB__)) + 0.175);
+ (*this)(5,3) = cos__q_jB__;
+ (*this)(5,4) = - sin__q_jB__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_com3::Type_fr_link0_X_com3()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_com3& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_com3::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+
+ (*this)(0,0) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(0,1) = (((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(0,2) = - sin__q_jA__;
+ (*this)(1,0) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(1,1) = (((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(1,2) = cos__q_jA__;
+ (*this)(2,0) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(2,1) = (( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__));
+ (*this)(3,0) = (((((( 0.495 * sin__q_jA__) * sin__q_jB__) + (((- 0.175 * sin__q_jA__) - ( 0.044 * cos__q_jA__)) * cos__q_jB__)) * sin__q_jC__) + ((((((- 0.175 * sin__q_jA__) - ( 0.044 * cos__q_jA__)) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.133 * sin__q_jA__));
+ (*this)(3,1) = (((((((( 0.175 * sin__q_jA__) + ( 0.044 * cos__q_jA__)) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) + (((- 0.175 * sin__q_jA__) - ( 0.044 * cos__q_jA__)) * cos__q_jB__)) * cos__q_jC__)) - ( 0.035 * sin__q_jA__));
+ (*this)(3,2) = ((((((( 0.133 * cos__q_jA__) * sin__q_jB__) + (( 0.035 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.035 * cos__q_jA__) * sin__q_jB__) - (( 0.133 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__));
+ (*this)(3,3) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(3,4) = (((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(3,5) = - sin__q_jA__;
+ (*this)(4,0) = ((((((( 0.175 * cos__q_jA__) - ( 0.044 * sin__q_jA__)) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + (((((( 0.175 * cos__q_jA__) - ( 0.044 * sin__q_jA__)) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.133 * cos__q_jA__));
+ (*this)(4,1) = (((((((( 0.044 * sin__q_jA__) - ( 0.175 * cos__q_jA__)) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) - ( 0.044 * sin__q_jA__)) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 0.035 * cos__q_jA__));
+ (*this)(4,2) = ((((((( 0.133 * sin__q_jA__) * sin__q_jB__) + (( 0.035 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.035 * sin__q_jA__) * sin__q_jB__) - (( 0.133 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__));
+ (*this)(4,3) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(4,4) = (((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(4,5) = cos__q_jA__;
+ (*this)(5,0) = ((( 0.044 * sin__q_jB__) * sin__q_jC__) - (( 0.044 * cos__q_jB__) * cos__q_jC__));
+ (*this)(5,1) = ((( 0.044 * cos__q_jB__) * sin__q_jC__) + (( 0.044 * sin__q_jB__) * cos__q_jC__));
+ (*this)(5,2) = (((((( 0.133 * cos__q_jB__) - ( 0.035 * sin__q_jB__)) * sin__q_jC__) + ((( 0.133 * sin__q_jB__) + ( 0.035 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175);
+ (*this)(5,3) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(5,4) = (( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__));
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_com4::Type_fr_link0_X_com4()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_com4& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_com4::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+
+ (*this)(0,0) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( sin__q_jA__ * cos__q_jD__));
+ (*this)(0,1) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__));
+ (*this)(0,2) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(1,0) = (((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( cos__q_jA__ * cos__q_jD__));
+ (*this)(1,1) = (( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__));
+ (*this)(1,2) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(2,0) = ((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * sin__q_jD__);
+ (*this)(2,1) = ((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__);
+ (*this)(2,2) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(3,0) = ((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 0.832 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.832 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.832 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * cos__q_jB__)) + ( 0.495 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(3,1) = ((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.832 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.832 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 0.832 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(3,2) = (((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__));
+ (*this)(3,3) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( sin__q_jA__ * cos__q_jD__));
+ (*this)(3,4) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__));
+ (*this)(3,5) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(4,0) = (((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 0.832 * cos__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.832 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.832 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * cos__q_jB__)) + ( 0.495 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(4,1) = ((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.832 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.832 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 0.832 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(4,2) = (((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__));
+ (*this)(4,3) = (((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( cos__q_jA__ * cos__q_jD__));
+ (*this)(4,4) = (( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__));
+ (*this)(4,5) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(5,0) = ((((((( 0.832 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + (((- 0.175 * sin__q_jB__) - ( 0.832 * cos__q_jB__)) * cos__q_jC__)) - ( 1.095 * sin__q_jB__)) - 0.175) * cos__q_jD__);
+ (*this)(5,1) = ((((((( 0.175 * cos__q_jB__) - ( 0.832 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 0.832 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__);
+ (*this)(5,3) = ((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * sin__q_jD__);
+ (*this)(5,4) = ((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__);
+ (*this)(5,5) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_com5::Type_fr_link0_X_com5()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_com5& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_com5::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+
+ (*this)(0,0) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(0,1) = ((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__));
+ (*this)(0,2) = (((((( 1.0 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(1,0) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(1,1) = ((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(1,2) = (((((( 1.0 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(2,0) = ((((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(2,1) = (((( cos__q_jB__ * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__));
+ (*this)(2,2) = (((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ (*this)(3,0) = ((((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * cos__q_jE__));
+ (*this)(3,1) = (((((((((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * cos__q_jC__)) + ( 0.175 * sin__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.07 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.07 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.07 * sin__q_jA__) * cos__q_jD__));
+ (*this)(3,2) = (((((((( 0.07 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.07 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.07 * sin__q_jA__) * sin__q_jD__) + ((((( 0.07 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.07 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(3,3) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(3,4) = ((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__));
+ (*this)(3,5) = (((((( 1.0 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(4,0) = ((((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * cos__q_jE__));
+ (*this)(4,1) = (((((((((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * cos__q_jC__)) - ( 0.175 * cos__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.07 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.07 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.07 * cos__q_jA__) * cos__q_jD__));
+ (*this)(4,2) = (((((((( 0.07 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.07 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.07 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.07 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.07 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(4,3) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(4,4) = ((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(4,5) = (((((( 1.0 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(5,0) = (((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * sin__q_jE__);
+ (*this)(5,1) = ((((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.07 * cos__q_jB__) * cos__q_jC__) - (( 0.07 * sin__q_jB__) * sin__q_jC__)) * sin__q_jD__));
+ (*this)(5,2) = ((((((- 0.07 * cos__q_jB__) * sin__q_jC__) - (( 0.07 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.07 * cos__q_jB__) * cos__q_jC__) - (( 0.07 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__));
+ (*this)(5,3) = ((((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(5,4) = (((( cos__q_jB__ * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__));
+ (*this)(5,5) = (((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_com6::Type_fr_link0_X_com6()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_com6& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_com6::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jF__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+ SCALAR cos__q_jF__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jF__ = TRAIT::sin( q(JF));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+ cos__q_jF__ = TRAIT::cos( q(JF));
+
+ (*this)(0,0) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,1) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(0,2) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(1,0) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(1,1) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,2) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(2,0) = ((((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + ((((( cos__q_jB__ * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(2,1) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(2,2) = ((((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(3,0) = (((((((((( 0.3429 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.3429 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.3429 * sin__q_jA__) * sin__q_jD__) + ((((( 0.3429 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.3429 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jF__) + ((((((((((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * cos__q_jC__)) + ( 0.175 * sin__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.3429 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.3429 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.3429 * sin__q_jA__) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(3,1) = (((((((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * sin__q_jE__) + ((((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 1.271 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * cos__q_jB__)) + ( 0.495 * cos__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + (((((- 0.3429 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.3429 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.3429 * sin__q_jA__) * cos__q_jD__)) * sin__q_jF__) + ((((((((( 0.3429 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.3429 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.3429 * sin__q_jA__) * sin__q_jD__) + ((((( 0.3429 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.3429 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(3,2) = ((((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * cos__q_jE__));
+ (*this)(3,3) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(3,4) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(3,5) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(4,0) = (((((((((( 0.3429 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.3429 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.3429 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.3429 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.3429 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jF__) + ((((((((((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * cos__q_jC__)) - ( 0.175 * cos__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.3429 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.3429 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.3429 * cos__q_jA__) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,1) = (((((((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * sin__q_jE__) + ((((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 1.271 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * cos__q_jB__)) + ( 0.495 * sin__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + (((((- 0.3429 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.3429 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.3429 * cos__q_jA__) * cos__q_jD__)) * sin__q_jF__) + ((((((((( 0.3429 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.3429 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.3429 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.3429 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.3429 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,2) = ((((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * cos__q_jE__));
+ (*this)(4,3) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(4,4) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,5) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(5,0) = ((((((((- 0.3429 * cos__q_jB__) * sin__q_jC__) - (( 0.3429 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.3429 * cos__q_jB__) * cos__q_jC__) - (( 0.3429 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__)) * sin__q_jF__) + (((((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.3429 * cos__q_jB__) * cos__q_jC__) - (( 0.3429 * sin__q_jB__) * sin__q_jC__)) * sin__q_jD__)) * cos__q_jF__));
+ (*this)(5,1) = ((((((((((( 1.271 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + (((- 0.175 * sin__q_jB__) - ( 1.271 * cos__q_jB__)) * cos__q_jC__)) - ( 1.095 * sin__q_jB__)) - 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.3429 * sin__q_jB__) * sin__q_jC__) - (( 0.3429 * cos__q_jB__) * cos__q_jC__)) * sin__q_jD__)) * sin__q_jF__) + (((((((- 0.3429 * cos__q_jB__) * sin__q_jC__) - (( 0.3429 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.3429 * cos__q_jB__) * cos__q_jC__) - (( 0.3429 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(5,2) = (((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * sin__q_jE__);
+ (*this)(5,3) = ((((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + ((((( cos__q_jB__ * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(5,4) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(5,5) = ((((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link1_X_fr_link0::Type_fr_link1_X_fr_link0()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link1_X_fr_link0& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link1_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+
+ (*this)(0,0) = cos__q_jA__;
+ (*this)(0,1) = sin__q_jA__;
+ (*this)(1,0) = - sin__q_jA__;
+ (*this)(1,1) = cos__q_jA__;
+ (*this)(3,3) = cos__q_jA__;
+ (*this)(3,4) = sin__q_jA__;
+ (*this)(4,3) = - sin__q_jA__;
+ (*this)(4,4) = cos__q_jA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link2_X_fr_link0::Type_fr_link2_X_fr_link0()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,2) = 0.175;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link2_X_fr_link0& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link2_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+
+ (*this)(0,0) = ( cos__q_jA__ * sin__q_jB__);
+ (*this)(0,1) = ( sin__q_jA__ * sin__q_jB__);
+ (*this)(0,2) = cos__q_jB__;
+ (*this)(1,0) = ( cos__q_jA__ * cos__q_jB__);
+ (*this)(1,1) = ( sin__q_jA__ * cos__q_jB__);
+ (*this)(1,2) = - sin__q_jB__;
+ (*this)(2,0) = - sin__q_jA__;
+ (*this)(2,1) = cos__q_jA__;
+ (*this)(3,0) = ((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__));
+ (*this)(3,1) = ((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__));
+ (*this)(3,3) = ( cos__q_jA__ * sin__q_jB__);
+ (*this)(3,4) = ( sin__q_jA__ * sin__q_jB__);
+ (*this)(3,5) = cos__q_jB__;
+ (*this)(4,0) = (((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__));
+ (*this)(4,1) = ((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__));
+ (*this)(4,3) = ( cos__q_jA__ * cos__q_jB__);
+ (*this)(4,4) = ( sin__q_jA__ * cos__q_jB__);
+ (*this)(4,5) = - sin__q_jB__;
+ (*this)(5,0) = (- 0.495 * cos__q_jA__);
+ (*this)(5,1) = (- 0.495 * sin__q_jA__);
+ (*this)(5,3) = - sin__q_jA__;
+ (*this)(5,4) = cos__q_jA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link3_X_fr_link0::Type_fr_link3_X_fr_link0()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link3_X_fr_link0& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link3_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+
+ (*this)(0,0) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(0,1) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(0,2) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(1,0) = (((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(1,1) = (((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(1,2) = (( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__));
+ (*this)(2,0) = - sin__q_jA__;
+ (*this)(2,1) = cos__q_jA__;
+ (*this)(3,0) = ((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__));
+ (*this)(3,1) = ((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__));
+ (*this)(3,3) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(3,4) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(3,5) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(4,0) = (((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__));
+ (*this)(4,1) = ((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__));
+ (*this)(4,3) = (((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(4,4) = (((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(4,5) = (( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__));
+ (*this)(5,0) = (((- 1.095 * cos__q_jA__) * cos__q_jB__) - ( 0.495 * cos__q_jA__));
+ (*this)(5,1) = (((- 1.095 * sin__q_jA__) * cos__q_jB__) - ( 0.495 * sin__q_jA__));
+ (*this)(5,2) = (( 1.095 * sin__q_jB__) + 0.175);
+ (*this)(5,3) = - sin__q_jA__;
+ (*this)(5,4) = cos__q_jA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link4_X_fr_link0::Type_fr_link4_X_fr_link0()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link4_X_fr_link0& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link4_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+
+ (*this)(0,0) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( sin__q_jA__ * cos__q_jD__));
+ (*this)(0,1) = (((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( cos__q_jA__ * cos__q_jD__));
+ (*this)(0,2) = (((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * sin__q_jD__);
+ (*this)(1,0) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__));
+ (*this)(1,1) = (( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__));
+ (*this)(1,2) = (((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__);
+ (*this)(2,0) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(2,1) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(2,2) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(3,0) = ((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 0.332 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.332 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.332 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * cos__q_jB__)) + ( 0.495 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(3,1) = (((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 0.332 * cos__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.332 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.332 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * cos__q_jB__)) + ( 0.495 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(3,2) = ((((((( 0.332 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + (((- 0.175 * sin__q_jB__) - ( 0.332 * cos__q_jB__)) * cos__q_jC__)) - ( 1.095 * sin__q_jB__)) - 0.175) * cos__q_jD__);
+ (*this)(3,3) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( sin__q_jA__ * cos__q_jD__));
+ (*this)(3,4) = (((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( cos__q_jA__ * cos__q_jD__));
+ (*this)(3,5) = (((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * sin__q_jD__);
+ (*this)(4,0) = ((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.332 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.332 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 0.332 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(4,1) = ((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.332 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.332 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 0.332 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(4,2) = ((((((( 0.175 * cos__q_jB__) - ( 0.332 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 0.332 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__);
+ (*this)(4,3) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__));
+ (*this)(4,4) = (( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__));
+ (*this)(4,5) = (((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__);
+ (*this)(5,0) = (((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__));
+ (*this)(5,1) = (((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__));
+ (*this)(5,3) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(5,4) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(5,5) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link5_X_fr_link0::Type_fr_link5_X_fr_link0()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link5_X_fr_link0& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link5_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+
+ (*this)(0,0) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(0,1) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(0,2) = (((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(1,0) = ((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__));
+ (*this)(1,1) = ((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(1,2) = ((((( 1.0 * cos__q_jB__) * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__));
+ (*this)(2,0) = ((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(2,1) = ((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(2,2) = (((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ (*this)(3,0) = ((((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * cos__q_jE__));
+ (*this)(3,1) = ((((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * cos__q_jE__));
+ (*this)(3,2) = (((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * sin__q_jE__);
+ (*this)(3,3) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(3,4) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(3,5) = (((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(4,0) = (((((((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * cos__q_jC__)) + ( 0.175 * sin__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(4,1) = (((((((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * cos__q_jC__)) - ( 0.175 * cos__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(4,2) = (((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * cos__q_jE__);
+ (*this)(4,3) = ((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__));
+ (*this)(4,4) = ((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(4,5) = ((((( 1.0 * cos__q_jB__) * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__));
+ (*this)(5,0) = (((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(5,1) = ((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(5,2) = ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__);
+ (*this)(5,3) = ((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(5,4) = ((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(5,5) = (((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link6_X_fr_link0::Type_fr_link6_X_fr_link0()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link6_X_fr_link0& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link6_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jF__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+ SCALAR cos__q_jF__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jF__ = TRAIT::sin( q(JF));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+ cos__q_jF__ = TRAIT::cos( q(JF));
+
+ (*this)(0,0) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,1) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,2) = (((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + (((((( 1.0 * cos__q_jB__) * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(1,0) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,1) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,2) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(2,0) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(2,1) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(2,2) = (((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(3,0) = (((((((((( 0.243 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.243 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.243 * sin__q_jA__) * sin__q_jD__) + ((((( 0.243 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jF__) + ((((((((((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * cos__q_jC__)) + ( 0.175 * sin__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.243 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.243 * sin__q_jA__) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(3,1) = (((((((((( 0.243 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.243 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.243 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.243 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jF__) + ((((((((((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * cos__q_jC__)) - ( 0.175 * cos__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.243 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.243 * cos__q_jA__) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(3,2) = ((((((((- 0.243 * cos__q_jB__) * sin__q_jC__) - (( 0.243 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.243 * cos__q_jB__) * cos__q_jC__) - (( 0.243 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__)) * sin__q_jF__) + (((((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.243 * cos__q_jB__) * cos__q_jC__) - (( 0.243 * sin__q_jB__) * sin__q_jC__)) * sin__q_jD__)) * cos__q_jF__));
+ (*this)(3,3) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(3,4) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(3,5) = (((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + (((((( 1.0 * cos__q_jB__) * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(4,0) = (((((((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * sin__q_jE__) + ((((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 1.271 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * cos__q_jB__)) + ( 0.495 * cos__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + (((((- 0.243 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.243 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.243 * sin__q_jA__) * cos__q_jD__)) * sin__q_jF__) + ((((((((( 0.243 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.243 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.243 * sin__q_jA__) * sin__q_jD__) + ((((( 0.243 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,1) = (((((((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * sin__q_jE__) + ((((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 1.271 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * cos__q_jB__)) + ( 0.495 * sin__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + (((((- 0.243 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.243 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.243 * cos__q_jA__) * cos__q_jD__)) * sin__q_jF__) + ((((((((( 0.243 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.243 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.243 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.243 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,2) = ((((((((((( 1.271 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + (((- 0.175 * sin__q_jB__) - ( 1.271 * cos__q_jB__)) * cos__q_jC__)) - ( 1.095 * sin__q_jB__)) - 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.243 * sin__q_jB__) * sin__q_jC__) - (( 0.243 * cos__q_jB__) * cos__q_jC__)) * sin__q_jD__)) * sin__q_jF__) + (((((((- 0.243 * cos__q_jB__) * sin__q_jC__) - (( 0.243 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.243 * cos__q_jB__) * cos__q_jC__) - (( 0.243 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,3) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,4) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,5) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(5,0) = ((((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * cos__q_jE__));
+ (*this)(5,1) = ((((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * cos__q_jE__));
+ (*this)(5,2) = (((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * sin__q_jE__);
+ (*this)(5,3) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(5,4) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(5,5) = (((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_ee_X_fr_link0::Type_fr_ee_X_fr_link0()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_ee_X_fr_link0& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_ee_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jF__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+ SCALAR cos__q_jF__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jF__ = TRAIT::sin( q(JF));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+ cos__q_jF__ = TRAIT::cos( q(JF));
+
+ (*this)(0,0) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,1) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,2) = (((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + (((((( 1.0 * cos__q_jB__) * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(1,0) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,1) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,2) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(2,0) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(2,1) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(2,2) = (((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(3,0) = (((((((((( 0.443 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.443 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.443 * sin__q_jA__) * sin__q_jD__) + ((((( 0.443 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.443 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jF__) + ((((((((((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * cos__q_jC__)) + ( 0.175 * sin__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.443 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.443 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.443 * sin__q_jA__) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(3,1) = (((((((((( 0.443 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.443 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.443 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.443 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.443 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jF__) + ((((((((((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * cos__q_jC__)) - ( 0.175 * cos__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.443 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.443 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.443 * cos__q_jA__) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(3,2) = ((((((((- 0.443 * cos__q_jB__) * sin__q_jC__) - (( 0.443 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.443 * cos__q_jB__) * cos__q_jC__) - (( 0.443 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__)) * sin__q_jF__) + (((((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.443 * cos__q_jB__) * cos__q_jC__) - (( 0.443 * sin__q_jB__) * sin__q_jC__)) * sin__q_jD__)) * cos__q_jF__));
+ (*this)(3,3) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(3,4) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(3,5) = (((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + (((((( 1.0 * cos__q_jB__) * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(4,0) = (((((((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * sin__q_jE__) + ((((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 1.271 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * cos__q_jB__)) + ( 0.495 * cos__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + (((((- 0.443 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.443 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.443 * sin__q_jA__) * cos__q_jD__)) * sin__q_jF__) + ((((((((( 0.443 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.443 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.443 * sin__q_jA__) * sin__q_jD__) + ((((( 0.443 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.443 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,1) = (((((((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * sin__q_jE__) + ((((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 1.271 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * cos__q_jB__)) + ( 0.495 * sin__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + (((((- 0.443 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.443 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.443 * cos__q_jA__) * cos__q_jD__)) * sin__q_jF__) + ((((((((( 0.443 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.443 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.443 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.443 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.443 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,2) = ((((((((((( 1.271 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + (((- 0.175 * sin__q_jB__) - ( 1.271 * cos__q_jB__)) * cos__q_jC__)) - ( 1.095 * sin__q_jB__)) - 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.443 * sin__q_jB__) * sin__q_jC__) - (( 0.443 * cos__q_jB__) * cos__q_jC__)) * sin__q_jD__)) * sin__q_jF__) + (((((((- 0.443 * cos__q_jB__) * sin__q_jC__) - (( 0.443 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.443 * cos__q_jB__) * cos__q_jC__) - (( 0.443 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,3) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,4) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,5) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(5,0) = ((((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * cos__q_jE__));
+ (*this)(5,1) = ((((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * cos__q_jE__));
+ (*this)(5,2) = (((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * sin__q_jE__);
+ (*this)(5,3) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(5,4) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(5,5) = (((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_com0_X_fr_link0::Type_com0_X_fr_link0()
+{
+ (*this)(0,0) = 1;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 1;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0.075;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = - 0.075;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = 1;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_com0_X_fr_link0& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_com0_X_fr_link0::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_com1_X_fr_link0::Type_com1_X_fr_link0()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1.0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,2) = 0.09;
+ (*this)(4,5) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_com1_X_fr_link0& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_com1_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+
+ (*this)(0,0) = cos__q_jA__;
+ (*this)(0,1) = sin__q_jA__;
+ (*this)(1,0) = - sin__q_jA__;
+ (*this)(1,1) = cos__q_jA__;
+ (*this)(3,0) = (- 0.25 * sin__q_jA__);
+ (*this)(3,1) = ( 0.25 * cos__q_jA__);
+ (*this)(3,3) = cos__q_jA__;
+ (*this)(3,4) = sin__q_jA__;
+ (*this)(4,0) = (- 0.25 * cos__q_jA__);
+ (*this)(4,1) = (- 0.25 * sin__q_jA__);
+ (*this)(4,3) = - sin__q_jA__;
+ (*this)(4,4) = cos__q_jA__;
+ (*this)(5,0) = ( 0.09 * sin__q_jA__);
+ (*this)(5,1) = (- 0.09 * cos__q_jA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_com2_X_fr_link0::Type_com2_X_fr_link0()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_com2_X_fr_link0& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_com2_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+
+ (*this)(0,0) = ( cos__q_jA__ * sin__q_jB__);
+ (*this)(0,1) = ( sin__q_jA__ * sin__q_jB__);
+ (*this)(0,2) = cos__q_jB__;
+ (*this)(1,0) = ( cos__q_jA__ * cos__q_jB__);
+ (*this)(1,1) = ( sin__q_jA__ * cos__q_jB__);
+ (*this)(1,2) = - sin__q_jB__;
+ (*this)(2,0) = - sin__q_jA__;
+ (*this)(2,1) = cos__q_jA__;
+ (*this)(3,0) = ((((- 0.495 * sin__q_jA__) * sin__q_jB__) + ((( 0.175 * sin__q_jA__) - ( 0.17 * cos__q_jA__)) * cos__q_jB__)) + ( 0.04 * sin__q_jA__));
+ (*this)(3,1) = (((( 0.495 * cos__q_jA__) * sin__q_jB__) + (((- 0.17 * sin__q_jA__) - ( 0.175 * cos__q_jA__)) * cos__q_jB__)) - ( 0.04 * cos__q_jA__));
+ (*this)(3,2) = ( 0.17 * sin__q_jB__);
+ (*this)(3,3) = ( cos__q_jA__ * sin__q_jB__);
+ (*this)(3,4) = ( sin__q_jA__ * sin__q_jB__);
+ (*this)(3,5) = cos__q_jB__;
+ (*this)(4,0) = ((((( 0.17 * cos__q_jA__) - ( 0.175 * sin__q_jA__)) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 0.47 * sin__q_jA__));
+ (*this)(4,1) = ((((( 0.17 * sin__q_jA__) + ( 0.175 * cos__q_jA__)) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 0.47 * cos__q_jA__));
+ (*this)(4,2) = ( 0.17 * cos__q_jB__);
+ (*this)(4,3) = ( cos__q_jA__ * cos__q_jB__);
+ (*this)(4,4) = ( sin__q_jA__ * cos__q_jB__);
+ (*this)(4,5) = - sin__q_jB__;
+ (*this)(5,0) = (((( 0.04 * cos__q_jA__) * sin__q_jB__) - (( 0.47 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__));
+ (*this)(5,1) = (((( 0.04 * sin__q_jA__) * sin__q_jB__) - (( 0.47 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__));
+ (*this)(5,2) = ((( 0.47 * sin__q_jB__) + ( 0.04 * cos__q_jB__)) + 0.175);
+ (*this)(5,3) = - sin__q_jA__;
+ (*this)(5,4) = cos__q_jA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_com3_X_fr_link0::Type_com3_X_fr_link0()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_com3_X_fr_link0& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_com3_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+
+ (*this)(0,0) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(0,1) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(0,2) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(1,0) = (((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(1,1) = (((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(1,2) = (( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__));
+ (*this)(2,0) = - sin__q_jA__;
+ (*this)(2,1) = cos__q_jA__;
+ (*this)(3,0) = (((((( 0.495 * sin__q_jA__) * sin__q_jB__) + (((- 0.175 * sin__q_jA__) - ( 0.044 * cos__q_jA__)) * cos__q_jB__)) * sin__q_jC__) + ((((((- 0.175 * sin__q_jA__) - ( 0.044 * cos__q_jA__)) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.133 * sin__q_jA__));
+ (*this)(3,1) = ((((((( 0.175 * cos__q_jA__) - ( 0.044 * sin__q_jA__)) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + (((((( 0.175 * cos__q_jA__) - ( 0.044 * sin__q_jA__)) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.133 * cos__q_jA__));
+ (*this)(3,2) = ((( 0.044 * sin__q_jB__) * sin__q_jC__) - (( 0.044 * cos__q_jB__) * cos__q_jC__));
+ (*this)(3,3) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(3,4) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(3,5) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(4,0) = (((((((( 0.175 * sin__q_jA__) + ( 0.044 * cos__q_jA__)) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) + (((- 0.175 * sin__q_jA__) - ( 0.044 * cos__q_jA__)) * cos__q_jB__)) * cos__q_jC__)) - ( 0.035 * sin__q_jA__));
+ (*this)(4,1) = (((((((( 0.044 * sin__q_jA__) - ( 0.175 * cos__q_jA__)) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) - ( 0.044 * sin__q_jA__)) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 0.035 * cos__q_jA__));
+ (*this)(4,2) = ((( 0.044 * cos__q_jB__) * sin__q_jC__) + (( 0.044 * sin__q_jB__) * cos__q_jC__));
+ (*this)(4,3) = (((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(4,4) = (((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(4,5) = (( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__));
+ (*this)(5,0) = ((((((( 0.133 * cos__q_jA__) * sin__q_jB__) + (( 0.035 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.035 * cos__q_jA__) * sin__q_jB__) - (( 0.133 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__));
+ (*this)(5,1) = ((((((( 0.133 * sin__q_jA__) * sin__q_jB__) + (( 0.035 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.035 * sin__q_jA__) * sin__q_jB__) - (( 0.133 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__));
+ (*this)(5,2) = (((((( 0.133 * cos__q_jB__) - ( 0.035 * sin__q_jB__)) * sin__q_jC__) + ((( 0.133 * sin__q_jB__) + ( 0.035 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175);
+ (*this)(5,3) = - sin__q_jA__;
+ (*this)(5,4) = cos__q_jA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_com4_X_fr_link0::Type_com4_X_fr_link0()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_com4_X_fr_link0& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_com4_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+
+ (*this)(0,0) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( sin__q_jA__ * cos__q_jD__));
+ (*this)(0,1) = (((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( cos__q_jA__ * cos__q_jD__));
+ (*this)(0,2) = (((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * sin__q_jD__);
+ (*this)(1,0) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__));
+ (*this)(1,1) = (( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__));
+ (*this)(1,2) = (((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__);
+ (*this)(2,0) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(2,1) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(2,2) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(3,0) = ((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 0.832 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.832 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.832 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * cos__q_jB__)) + ( 0.495 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(3,1) = (((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 0.832 * cos__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.832 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.832 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * cos__q_jB__)) + ( 0.495 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(3,2) = ((((((( 0.832 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + (((- 0.175 * sin__q_jB__) - ( 0.832 * cos__q_jB__)) * cos__q_jC__)) - ( 1.095 * sin__q_jB__)) - 0.175) * cos__q_jD__);
+ (*this)(3,3) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( sin__q_jA__ * cos__q_jD__));
+ (*this)(3,4) = (((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( cos__q_jA__ * cos__q_jD__));
+ (*this)(3,5) = (((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * sin__q_jD__);
+ (*this)(4,0) = ((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.832 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.832 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 0.832 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(4,1) = ((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.832 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.832 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 0.832 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(4,2) = ((((((( 0.175 * cos__q_jB__) - ( 0.832 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 0.832 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__);
+ (*this)(4,3) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__));
+ (*this)(4,4) = (( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__));
+ (*this)(4,5) = (((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__);
+ (*this)(5,0) = (((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__));
+ (*this)(5,1) = (((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__));
+ (*this)(5,3) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(5,4) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(5,5) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_com5_X_fr_link0::Type_com5_X_fr_link0()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_com5_X_fr_link0& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_com5_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+
+ (*this)(0,0) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(0,1) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(0,2) = (((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(1,0) = ((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__));
+ (*this)(1,1) = ((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(1,2) = ((((( 1.0 * cos__q_jB__) * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__));
+ (*this)(2,0) = ((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(2,1) = ((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(2,2) = (((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ (*this)(3,0) = ((((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * cos__q_jE__));
+ (*this)(3,1) = ((((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * cos__q_jE__));
+ (*this)(3,2) = (((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * sin__q_jE__);
+ (*this)(3,3) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(3,4) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(3,5) = (((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(4,0) = (((((((((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * cos__q_jC__)) + ( 0.175 * sin__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.07 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.07 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.07 * sin__q_jA__) * cos__q_jD__));
+ (*this)(4,1) = (((((((((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * cos__q_jC__)) - ( 0.175 * cos__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.07 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.07 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.07 * cos__q_jA__) * cos__q_jD__));
+ (*this)(4,2) = ((((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.07 * cos__q_jB__) * cos__q_jC__) - (( 0.07 * sin__q_jB__) * sin__q_jC__)) * sin__q_jD__));
+ (*this)(4,3) = ((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__));
+ (*this)(4,4) = ((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(4,5) = ((((( 1.0 * cos__q_jB__) * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__));
+ (*this)(5,0) = (((((((( 0.07 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.07 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.07 * sin__q_jA__) * sin__q_jD__) + ((((( 0.07 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.07 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(5,1) = (((((((( 0.07 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.07 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.07 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.07 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.07 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(5,2) = ((((((- 0.07 * cos__q_jB__) * sin__q_jC__) - (( 0.07 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.07 * cos__q_jB__) * cos__q_jC__) - (( 0.07 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__));
+ (*this)(5,3) = ((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(5,4) = ((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(5,5) = (((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_com6_X_fr_link0::Type_com6_X_fr_link0()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_com6_X_fr_link0& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_com6_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jF__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+ SCALAR cos__q_jF__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jF__ = TRAIT::sin( q(JF));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+ cos__q_jF__ = TRAIT::cos( q(JF));
+
+ (*this)(0,0) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,1) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,2) = (((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + (((((( 1.0 * cos__q_jB__) * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(1,0) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,1) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,2) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(2,0) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(2,1) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(2,2) = (((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(3,0) = (((((((((( 0.3429 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.3429 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.3429 * sin__q_jA__) * sin__q_jD__) + ((((( 0.3429 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.3429 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jF__) + ((((((((((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * cos__q_jC__)) + ( 0.175 * sin__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.3429 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.3429 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.3429 * sin__q_jA__) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(3,1) = (((((((((( 0.3429 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.3429 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.3429 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.3429 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.3429 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jF__) + ((((((((((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * cos__q_jC__)) - ( 0.175 * cos__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.3429 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.3429 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.3429 * cos__q_jA__) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(3,2) = ((((((((- 0.3429 * cos__q_jB__) * sin__q_jC__) - (( 0.3429 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.3429 * cos__q_jB__) * cos__q_jC__) - (( 0.3429 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__)) * sin__q_jF__) + (((((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.3429 * cos__q_jB__) * cos__q_jC__) - (( 0.3429 * sin__q_jB__) * sin__q_jC__)) * sin__q_jD__)) * cos__q_jF__));
+ (*this)(3,3) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(3,4) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(3,5) = (((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + (((((( 1.0 * cos__q_jB__) * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(4,0) = (((((((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * sin__q_jE__) + ((((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 1.271 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * cos__q_jB__)) + ( 0.495 * cos__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + (((((- 0.3429 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.3429 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.3429 * sin__q_jA__) * cos__q_jD__)) * sin__q_jF__) + ((((((((( 0.3429 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.3429 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.3429 * sin__q_jA__) * sin__q_jD__) + ((((( 0.3429 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.3429 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,1) = (((((((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * sin__q_jE__) + ((((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 1.271 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * cos__q_jB__)) + ( 0.495 * sin__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + (((((- 0.3429 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.3429 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.3429 * cos__q_jA__) * cos__q_jD__)) * sin__q_jF__) + ((((((((( 0.3429 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.3429 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.3429 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.3429 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.3429 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,2) = ((((((((((( 1.271 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + (((- 0.175 * sin__q_jB__) - ( 1.271 * cos__q_jB__)) * cos__q_jC__)) - ( 1.095 * sin__q_jB__)) - 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.3429 * sin__q_jB__) * sin__q_jC__) - (( 0.3429 * cos__q_jB__) * cos__q_jC__)) * sin__q_jD__)) * sin__q_jF__) + (((((((- 0.3429 * cos__q_jB__) * sin__q_jC__) - (( 0.3429 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.3429 * cos__q_jB__) * cos__q_jC__) - (( 0.3429 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,3) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,4) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,5) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(5,0) = ((((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * cos__q_jE__));
+ (*this)(5,1) = ((((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * cos__q_jE__));
+ (*this)(5,2) = (((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * sin__q_jE__);
+ (*this)(5,3) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(5,4) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(5,5) = (((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_jA::Type_fr_link0_X_fr_jA()
+{
+ (*this)(0,0) = 1;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 1.0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1.0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = 1.0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_jA& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_jA::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_jB::Type_fr_link0_X_fr_jB()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,3) = 0;
+ (*this)(4,3) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0.175;
+ (*this)(5,3) = 1.0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_jB& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_jB::update(const JState& q) {
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+
+ (*this)(0,1) = cos__q_jA__;
+ (*this)(0,2) = - sin__q_jA__;
+ (*this)(1,1) = sin__q_jA__;
+ (*this)(1,2) = cos__q_jA__;
+ (*this)(3,0) = ( 0.175 * sin__q_jA__);
+ (*this)(3,1) = (- 0.495 * sin__q_jA__);
+ (*this)(3,2) = (- 0.495 * cos__q_jA__);
+ (*this)(3,4) = cos__q_jA__;
+ (*this)(3,5) = - sin__q_jA__;
+ (*this)(4,0) = (- 0.175 * cos__q_jA__);
+ (*this)(4,1) = ( 0.495 * cos__q_jA__);
+ (*this)(4,2) = (- 0.495 * sin__q_jA__);
+ (*this)(4,4) = sin__q_jA__;
+ (*this)(4,5) = cos__q_jA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_jC::Type_fr_link0_X_fr_jC()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_jC& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_jC::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+
+ (*this)(0,0) = ( cos__q_jA__ * cos__q_jB__);
+ (*this)(0,1) = (- cos__q_jA__ * sin__q_jB__);
+ (*this)(0,2) = - sin__q_jA__;
+ (*this)(1,0) = ( sin__q_jA__ * cos__q_jB__);
+ (*this)(1,1) = (- sin__q_jA__ * sin__q_jB__);
+ (*this)(1,2) = cos__q_jA__;
+ (*this)(2,0) = - sin__q_jB__;
+ (*this)(2,1) = - cos__q_jB__;
+ (*this)(3,0) = ((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__));
+ (*this)(3,1) = ((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__));
+ (*this)(3,2) = (((- 1.095 * cos__q_jA__) * cos__q_jB__) - ( 0.495 * cos__q_jA__));
+ (*this)(3,3) = ( cos__q_jA__ * cos__q_jB__);
+ (*this)(3,4) = (- cos__q_jA__ * sin__q_jB__);
+ (*this)(3,5) = - sin__q_jA__;
+ (*this)(4,0) = (((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__));
+ (*this)(4,1) = ((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__));
+ (*this)(4,2) = (((- 1.095 * sin__q_jA__) * cos__q_jB__) - ( 0.495 * sin__q_jA__));
+ (*this)(4,3) = ( sin__q_jA__ * cos__q_jB__);
+ (*this)(4,4) = (- sin__q_jA__ * sin__q_jB__);
+ (*this)(4,5) = cos__q_jA__;
+ (*this)(5,2) = (( 1.095 * sin__q_jB__) + 0.175);
+ (*this)(5,3) = - sin__q_jB__;
+ (*this)(5,4) = - cos__q_jB__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_jD::Type_fr_link0_X_fr_jD()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_jD& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_jD::update(const JState& q) {
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+
+ (*this)(0,0) = sin__q_jA__;
+ (*this)(0,1) = (((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(0,2) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(1,0) = - cos__q_jA__;
+ (*this)(1,1) = (((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(1,2) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(2,1) = (( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__));
+ (*this)(2,2) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(3,0) = (((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.332 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.332 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * cos__q_jB__)) + ( 0.495 * cos__q_jA__));
+ (*this)(3,1) = ((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 0.332 * sin__q_jA__));
+ (*this)(3,2) = (((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__));
+ (*this)(3,3) = sin__q_jA__;
+ (*this)(3,4) = (((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(3,5) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(4,0) = (((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.332 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.332 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * cos__q_jB__)) + ( 0.495 * sin__q_jA__));
+ (*this)(4,1) = (((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 0.332 * cos__q_jA__));
+ (*this)(4,2) = (((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__));
+ (*this)(4,3) = - cos__q_jA__;
+ (*this)(4,4) = (((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(4,5) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(5,0) = (((((( 0.332 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + (((- 0.175 * sin__q_jB__) - ( 0.332 * cos__q_jB__)) * cos__q_jC__)) - ( 1.095 * sin__q_jB__)) - 0.175);
+ (*this)(5,4) = (( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__));
+ (*this)(5,5) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_jE::Type_fr_link0_X_fr_jE()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(5,0) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_jE& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_jE::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jD__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+
+ (*this)(0,0) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(0,1) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__));
+ (*this)(0,2) = ((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(1,0) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(1,1) = (( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__));
+ (*this)(1,2) = ((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(2,0) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(2,1) = ((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__);
+ (*this)(2,2) = ((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ (*this)(3,0) = (((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__));
+ (*this)(3,1) = ((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(3,2) = (((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(3,3) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(3,4) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__));
+ (*this)(3,5) = (((((( 1.0 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(4,0) = (((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__));
+ (*this)(4,1) = ((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(4,2) = ((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(4,3) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(4,4) = (( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__));
+ (*this)(4,5) = (((((( 1.0 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(5,1) = ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__);
+ (*this)(5,2) = ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__);
+ (*this)(5,3) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(5,4) = (((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__);
+ (*this)(5,5) = (((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_jF::Type_fr_link0_X_fr_jF()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_jF& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link0_X_fr_jF::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jE__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jD__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+
+ (*this)(0,0) = ((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__));
+ (*this)(0,1) = (((((( 1.0 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(0,2) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(1,0) = ((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(1,1) = (((((( 1.0 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(1,2) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(2,0) = (((( cos__q_jB__ * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__));
+ (*this)(2,1) = (((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ (*this)(2,2) = ((((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(3,0) = (((((((((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * cos__q_jC__)) + ( 0.175 * sin__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.243 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.243 * sin__q_jA__) * cos__q_jD__));
+ (*this)(3,1) = (((((((( 0.243 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.243 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.243 * sin__q_jA__) * sin__q_jD__) + ((((( 0.243 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(3,2) = ((((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * cos__q_jE__));
+ (*this)(3,3) = (((((( 1.0 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__));
+ (*this)(3,4) = (((((( 1.0 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(3,5) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(4,0) = (((((((((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * cos__q_jC__)) - ( 0.175 * cos__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.243 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.243 * cos__q_jA__) * cos__q_jD__));
+ (*this)(4,1) = (((((((( 0.243 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.243 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.243 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.243 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(4,2) = ((((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * cos__q_jE__));
+ (*this)(4,3) = (((((( 1.0 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + (((( 1.0 * cos__q_jA__) * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(4,4) = (((((( 1.0 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(4,5) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(5,0) = ((((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.243 * cos__q_jB__) * cos__q_jC__) - (( 0.243 * sin__q_jB__) * sin__q_jC__)) * sin__q_jD__));
+ (*this)(5,1) = ((((((- 0.243 * cos__q_jB__) * sin__q_jC__) - (( 0.243 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.243 * cos__q_jB__) * cos__q_jC__) - (( 0.243 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__));
+ (*this)(5,2) = (((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * sin__q_jE__);
+ (*this)(5,3) = ((((( 1.0 * cos__q_jB__) * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__));
+ (*this)(5,4) = (((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ (*this)(5,5) = ((((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link2_X_fr_link1::Type_fr_link2_X_fr_link1()
+{
+ (*this)(0,1) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,1) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 1.0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,4) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,4) = 0;
+ (*this)(5,0) = - 0.495;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0.175;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 1.0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link2_X_fr_link1& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link2_X_fr_link1::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR cos__q_jB__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+
+ (*this)(0,0) = sin__q_jB__;
+ (*this)(0,2) = cos__q_jB__;
+ (*this)(1,0) = cos__q_jB__;
+ (*this)(1,2) = - sin__q_jB__;
+ (*this)(3,1) = (( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__));
+ (*this)(3,3) = sin__q_jB__;
+ (*this)(3,5) = cos__q_jB__;
+ (*this)(4,1) = (( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__));
+ (*this)(4,3) = cos__q_jB__;
+ (*this)(4,5) = - sin__q_jB__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link1_X_fr_link2::Type_fr_link1_X_fr_link2()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 0;
+ (*this)(1,2) = 1.0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = - 0.495;
+ (*this)(3,5) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = 0;
+ (*this)(4,5) = 1.0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0.175;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link1_X_fr_link2& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link1_X_fr_link2::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR cos__q_jB__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+
+ (*this)(0,0) = sin__q_jB__;
+ (*this)(0,1) = cos__q_jB__;
+ (*this)(2,0) = cos__q_jB__;
+ (*this)(2,1) = - sin__q_jB__;
+ (*this)(3,3) = sin__q_jB__;
+ (*this)(3,4) = cos__q_jB__;
+ (*this)(4,0) = (( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__));
+ (*this)(4,1) = (( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__));
+ (*this)(5,3) = cos__q_jB__;
+ (*this)(5,4) = - sin__q_jB__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link3_X_fr_link2::Type_fr_link3_X_fr_link2()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1.0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = - 1.095;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link3_X_fr_link2& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link3_X_fr_link2::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR cos__q_jC__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+
+ (*this)(0,0) = - sin__q_jC__;
+ (*this)(0,1) = cos__q_jC__;
+ (*this)(1,0) = - cos__q_jC__;
+ (*this)(1,1) = - sin__q_jC__;
+ (*this)(3,2) = ( 1.095 * cos__q_jC__);
+ (*this)(3,3) = - sin__q_jC__;
+ (*this)(3,4) = cos__q_jC__;
+ (*this)(4,2) = (- 1.095 * sin__q_jC__);
+ (*this)(4,3) = - cos__q_jC__;
+ (*this)(4,4) = - sin__q_jC__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link2_X_fr_link3::Type_fr_link2_X_fr_link3()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = - 1.095;
+ (*this)(4,5) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link2_X_fr_link3& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link2_X_fr_link3::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR cos__q_jC__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+
+ (*this)(0,0) = - sin__q_jC__;
+ (*this)(0,1) = - cos__q_jC__;
+ (*this)(1,0) = cos__q_jC__;
+ (*this)(1,1) = - sin__q_jC__;
+ (*this)(3,3) = - sin__q_jC__;
+ (*this)(3,4) = - cos__q_jC__;
+ (*this)(4,3) = cos__q_jC__;
+ (*this)(4,4) = - sin__q_jC__;
+ (*this)(5,0) = ( 1.095 * cos__q_jC__);
+ (*this)(5,1) = (- 1.095 * sin__q_jC__);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link4_X_fr_link3::Type_fr_link4_X_fr_link3()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,3) = 0;
+ (*this)(4,3) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0.175;
+ (*this)(5,3) = 1.0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link4_X_fr_link3& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link4_X_fr_link3::update(const JState& q) {
+ SCALAR sin__q_jD__;
+ SCALAR cos__q_jD__;
+
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+
+ (*this)(0,1) = sin__q_jD__;
+ (*this)(0,2) = - cos__q_jD__;
+ (*this)(1,1) = cos__q_jD__;
+ (*this)(1,2) = sin__q_jD__;
+ (*this)(3,0) = ( 0.175 * cos__q_jD__);
+ (*this)(3,1) = ( 0.332 * cos__q_jD__);
+ (*this)(3,2) = ( 0.332 * sin__q_jD__);
+ (*this)(3,4) = sin__q_jD__;
+ (*this)(3,5) = - cos__q_jD__;
+ (*this)(4,0) = (- 0.175 * sin__q_jD__);
+ (*this)(4,1) = (- 0.332 * sin__q_jD__);
+ (*this)(4,2) = ( 0.332 * cos__q_jD__);
+ (*this)(4,4) = cos__q_jD__;
+ (*this)(4,5) = sin__q_jD__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link3_X_fr_link4::Type_fr_link3_X_fr_link4()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 1.0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 1.0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,2) = 0.175;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link3_X_fr_link4& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link3_X_fr_link4::update(const JState& q) {
+ SCALAR sin__q_jD__;
+ SCALAR cos__q_jD__;
+
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+
+ (*this)(1,0) = sin__q_jD__;
+ (*this)(1,1) = cos__q_jD__;
+ (*this)(2,0) = - cos__q_jD__;
+ (*this)(2,1) = sin__q_jD__;
+ (*this)(3,0) = ( 0.175 * cos__q_jD__);
+ (*this)(3,1) = (- 0.175 * sin__q_jD__);
+ (*this)(4,0) = ( 0.332 * cos__q_jD__);
+ (*this)(4,1) = (- 0.332 * sin__q_jD__);
+ (*this)(4,3) = sin__q_jD__;
+ (*this)(4,4) = cos__q_jD__;
+ (*this)(5,0) = ( 0.332 * sin__q_jD__);
+ (*this)(5,1) = ( 0.332 * cos__q_jD__);
+ (*this)(5,3) = - cos__q_jD__;
+ (*this)(5,4) = sin__q_jD__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link5_X_fr_link4::Type_fr_link5_X_fr_link4()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = - 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = - 0.939;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = - 1;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link5_X_fr_link4& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link5_X_fr_link4::update(const JState& q) {
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jE__;
+
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+
+ (*this)(0,1) = sin__q_jE__;
+ (*this)(0,2) = cos__q_jE__;
+ (*this)(1,1) = cos__q_jE__;
+ (*this)(1,2) = - sin__q_jE__;
+ (*this)(3,0) = (- 0.939 * sin__q_jE__);
+ (*this)(3,4) = sin__q_jE__;
+ (*this)(3,5) = cos__q_jE__;
+ (*this)(4,0) = (- 0.939 * cos__q_jE__);
+ (*this)(4,4) = cos__q_jE__;
+ (*this)(4,5) = - sin__q_jE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link4_X_fr_link5::Type_fr_link4_X_fr_link5()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = - 1;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = - 1.0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = - 0.939;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link4_X_fr_link5& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link4_X_fr_link5::update(const JState& q) {
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jE__;
+
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+
+ (*this)(1,0) = sin__q_jE__;
+ (*this)(1,1) = cos__q_jE__;
+ (*this)(2,0) = cos__q_jE__;
+ (*this)(2,1) = - sin__q_jE__;
+ (*this)(3,0) = (- 0.939 * sin__q_jE__);
+ (*this)(3,1) = (- 0.939 * cos__q_jE__);
+ (*this)(4,3) = sin__q_jE__;
+ (*this)(4,4) = cos__q_jE__;
+ (*this)(5,3) = cos__q_jE__;
+ (*this)(5,4) = - sin__q_jE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link6_X_fr_link5::Type_fr_link6_X_fr_link5()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 1;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,3) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,3) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 1;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link6_X_fr_link5& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link6_X_fr_link5::update(const JState& q) {
+ SCALAR sin__q_jF__;
+ SCALAR cos__q_jF__;
+
+ sin__q_jF__ = TRAIT::sin( q(JF));
+ cos__q_jF__ = TRAIT::cos( q(JF));
+
+ (*this)(0,1) = cos__q_jF__;
+ (*this)(0,2) = sin__q_jF__;
+ (*this)(1,1) = - sin__q_jF__;
+ (*this)(1,2) = cos__q_jF__;
+ (*this)(3,1) = (- 0.243 * sin__q_jF__);
+ (*this)(3,2) = ( 0.243 * cos__q_jF__);
+ (*this)(3,4) = cos__q_jF__;
+ (*this)(3,5) = sin__q_jF__;
+ (*this)(4,1) = (- 0.243 * cos__q_jF__);
+ (*this)(4,2) = (- 0.243 * sin__q_jF__);
+ (*this)(4,4) = - sin__q_jF__;
+ (*this)(4,5) = cos__q_jF__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link5_X_fr_link6::Type_fr_link5_X_fr_link6()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 1;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 1;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link5_X_fr_link6& iit::testirb4600::tpl::MotionTransforms<TRAIT>::Type_fr_link5_X_fr_link6::update(const JState& q) {
+ SCALAR sin__q_jF__;
+ SCALAR cos__q_jF__;
+
+ sin__q_jF__ = TRAIT::sin( q(JF));
+ cos__q_jF__ = TRAIT::cos( q(JF));
+
+ (*this)(1,0) = cos__q_jF__;
+ (*this)(1,1) = - sin__q_jF__;
+ (*this)(2,0) = sin__q_jF__;
+ (*this)(2,1) = cos__q_jF__;
+ (*this)(4,0) = (- 0.243 * sin__q_jF__);
+ (*this)(4,1) = (- 0.243 * cos__q_jF__);
+ (*this)(4,3) = cos__q_jF__;
+ (*this)(4,4) = - sin__q_jF__;
+ (*this)(5,0) = ( 0.243 * cos__q_jF__);
+ (*this)(5,1) = (- 0.243 * sin__q_jF__);
+ (*this)(5,3) = sin__q_jF__;
+ (*this)(5,4) = cos__q_jF__;
+ return *this;
+}
+
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_link1::Type_fr_link0_X_fr_link1()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_link1& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_link1::update(const JState& q) {
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+
+ (*this)(0,0) = cos__q_jA__;
+ (*this)(0,1) = - sin__q_jA__;
+ (*this)(1,0) = sin__q_jA__;
+ (*this)(1,1) = cos__q_jA__;
+ (*this)(3,3) = cos__q_jA__;
+ (*this)(3,4) = - sin__q_jA__;
+ (*this)(4,3) = sin__q_jA__;
+ (*this)(4,4) = cos__q_jA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_link2::Type_fr_link0_X_fr_link2()
+{
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0.175;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_link2& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_link2::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+
+ (*this)(0,0) = ( cos__q_jA__ * sin__q_jB__);
+ (*this)(0,1) = ( cos__q_jA__ * cos__q_jB__);
+ (*this)(0,2) = - sin__q_jA__;
+ (*this)(0,3) = ((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__));
+ (*this)(0,4) = (((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__));
+ (*this)(0,5) = (- 0.495 * cos__q_jA__);
+ (*this)(1,0) = ( sin__q_jA__ * sin__q_jB__);
+ (*this)(1,1) = ( sin__q_jA__ * cos__q_jB__);
+ (*this)(1,2) = cos__q_jA__;
+ (*this)(1,3) = ((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__));
+ (*this)(1,4) = ((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__));
+ (*this)(1,5) = (- 0.495 * sin__q_jA__);
+ (*this)(2,0) = cos__q_jB__;
+ (*this)(2,1) = - sin__q_jB__;
+ (*this)(3,3) = ( cos__q_jA__ * sin__q_jB__);
+ (*this)(3,4) = ( cos__q_jA__ * cos__q_jB__);
+ (*this)(3,5) = - sin__q_jA__;
+ (*this)(4,3) = ( sin__q_jA__ * sin__q_jB__);
+ (*this)(4,4) = ( sin__q_jA__ * cos__q_jB__);
+ (*this)(4,5) = cos__q_jA__;
+ (*this)(5,3) = cos__q_jB__;
+ (*this)(5,4) = - sin__q_jB__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_link3::Type_fr_link0_X_fr_link3()
+{
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_link3& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_link3::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+
+ (*this)(0,0) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(0,1) = (((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(0,2) = - sin__q_jA__;
+ (*this)(0,3) = ((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__));
+ (*this)(0,4) = (((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__));
+ (*this)(0,5) = (((- 1.095 * cos__q_jA__) * cos__q_jB__) - ( 0.495 * cos__q_jA__));
+ (*this)(1,0) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(1,1) = (((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(1,2) = cos__q_jA__;
+ (*this)(1,3) = ((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__));
+ (*this)(1,4) = ((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__));
+ (*this)(1,5) = (((- 1.095 * sin__q_jA__) * cos__q_jB__) - ( 0.495 * sin__q_jA__));
+ (*this)(2,0) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(2,1) = (( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__));
+ (*this)(2,5) = (( 1.095 * sin__q_jB__) + 0.175);
+ (*this)(3,3) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(3,4) = (((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(3,5) = - sin__q_jA__;
+ (*this)(4,3) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(4,4) = (((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(4,5) = cos__q_jA__;
+ (*this)(5,3) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(5,4) = (( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__));
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_link4::Type_fr_link0_X_fr_link4()
+{
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_link4& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_link4::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+
+ (*this)(0,0) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( sin__q_jA__ * cos__q_jD__));
+ (*this)(0,1) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__));
+ (*this)(0,2) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(0,3) = ((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 0.332 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.332 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.332 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * cos__q_jB__)) + ( 0.495 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(0,4) = ((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.332 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.332 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 0.332 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(0,5) = (((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__));
+ (*this)(1,0) = (((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( cos__q_jA__ * cos__q_jD__));
+ (*this)(1,1) = (( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__));
+ (*this)(1,2) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(1,3) = (((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 0.332 * cos__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.332 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.332 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * cos__q_jB__)) + ( 0.495 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(1,4) = ((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.332 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.332 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 0.332 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(1,5) = (((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__));
+ (*this)(2,0) = ((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * sin__q_jD__);
+ (*this)(2,1) = ((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__);
+ (*this)(2,2) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(2,3) = ((((((( 0.332 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + (((- 0.175 * sin__q_jB__) - ( 0.332 * cos__q_jB__)) * cos__q_jC__)) - ( 1.095 * sin__q_jB__)) - 0.175) * cos__q_jD__);
+ (*this)(2,4) = ((((((( 0.175 * cos__q_jB__) - ( 0.332 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 0.332 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__);
+ (*this)(3,3) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( sin__q_jA__ * cos__q_jD__));
+ (*this)(3,4) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__));
+ (*this)(3,5) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(4,3) = (((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( cos__q_jA__ * cos__q_jD__));
+ (*this)(4,4) = (( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__));
+ (*this)(4,5) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(5,3) = ((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * sin__q_jD__);
+ (*this)(5,4) = ((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__);
+ (*this)(5,5) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_link5::Type_fr_link0_X_fr_link5()
+{
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_link5& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_link5::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+
+ (*this)(0,0) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(0,1) = ((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__));
+ (*this)(0,2) = (((((( 1.0 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(0,3) = ((((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * cos__q_jE__));
+ (*this)(0,4) = (((((((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * cos__q_jC__)) + ( 0.175 * sin__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(0,5) = (((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(1,0) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(1,1) = ((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(1,2) = (((((( 1.0 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(1,3) = ((((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * cos__q_jE__));
+ (*this)(1,4) = (((((((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * cos__q_jC__)) - ( 0.175 * cos__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(1,5) = ((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(2,0) = ((((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(2,1) = (((( cos__q_jB__ * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__));
+ (*this)(2,2) = (((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ (*this)(2,3) = (((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * sin__q_jE__);
+ (*this)(2,4) = (((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * cos__q_jE__);
+ (*this)(2,5) = ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__);
+ (*this)(3,3) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(3,4) = ((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__));
+ (*this)(3,5) = ((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(4,3) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(4,4) = ((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(4,5) = ((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(5,3) = ((((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(5,4) = (((( cos__q_jB__ * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__));
+ (*this)(5,5) = ((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_link6::Type_fr_link0_X_fr_link6()
+{
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_link6& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_link6::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jF__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+ SCALAR cos__q_jF__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jF__ = TRAIT::sin( q(JF));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+ cos__q_jF__ = TRAIT::cos( q(JF));
+
+ (*this)(0,0) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,1) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(0,2) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(0,3) = (((((((((( 0.243 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.243 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.243 * sin__q_jA__) * sin__q_jD__) + ((((( 0.243 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jF__) + ((((((((((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * cos__q_jC__)) + ( 0.175 * sin__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.243 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.243 * sin__q_jA__) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(0,4) = (((((((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * sin__q_jE__) + ((((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 1.271 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * cos__q_jB__)) + ( 0.495 * cos__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + (((((- 0.243 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.243 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.243 * sin__q_jA__) * cos__q_jD__)) * sin__q_jF__) + ((((((((( 0.243 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.243 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.243 * sin__q_jA__) * sin__q_jD__) + ((((( 0.243 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(0,5) = ((((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * cos__q_jE__));
+ (*this)(1,0) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(1,1) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,2) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(1,3) = (((((((((( 0.243 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.243 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.243 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.243 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jF__) + ((((((((((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * cos__q_jC__)) - ( 0.175 * cos__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.243 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.243 * cos__q_jA__) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,4) = (((((((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * sin__q_jE__) + ((((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 1.271 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * cos__q_jB__)) + ( 0.495 * sin__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + (((((- 0.243 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.243 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.243 * cos__q_jA__) * cos__q_jD__)) * sin__q_jF__) + ((((((((( 0.243 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.243 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.243 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.243 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,5) = ((((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * cos__q_jE__));
+ (*this)(2,0) = ((((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + ((((( cos__q_jB__ * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(2,1) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(2,2) = ((((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(2,3) = ((((((((- 0.243 * cos__q_jB__) * sin__q_jC__) - (( 0.243 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.243 * cos__q_jB__) * cos__q_jC__) - (( 0.243 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__)) * sin__q_jF__) + (((((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.243 * cos__q_jB__) * cos__q_jC__) - (( 0.243 * sin__q_jB__) * sin__q_jC__)) * sin__q_jD__)) * cos__q_jF__));
+ (*this)(2,4) = ((((((((((( 1.271 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + (((- 0.175 * sin__q_jB__) - ( 1.271 * cos__q_jB__)) * cos__q_jC__)) - ( 1.095 * sin__q_jB__)) - 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.243 * sin__q_jB__) * sin__q_jC__) - (( 0.243 * cos__q_jB__) * cos__q_jC__)) * sin__q_jD__)) * sin__q_jF__) + (((((((- 0.243 * cos__q_jB__) * sin__q_jC__) - (( 0.243 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.243 * cos__q_jB__) * cos__q_jC__) - (( 0.243 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(2,5) = (((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * sin__q_jE__);
+ (*this)(3,3) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(3,4) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(3,5) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(4,3) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(4,4) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,5) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(5,3) = ((((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + ((((( cos__q_jB__ * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(5,4) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(5,5) = ((((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_ee::Type_fr_link0_X_fr_ee()
+{
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_ee& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_ee::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jF__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+ SCALAR cos__q_jF__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jF__ = TRAIT::sin( q(JF));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+ cos__q_jF__ = TRAIT::cos( q(JF));
+
+ (*this)(0,0) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,1) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(0,2) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(0,3) = (((((((((( 0.443 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.443 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.443 * sin__q_jA__) * sin__q_jD__) + ((((( 0.443 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.443 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jF__) + ((((((((((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * cos__q_jC__)) + ( 0.175 * sin__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.443 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.443 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.443 * sin__q_jA__) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(0,4) = (((((((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * sin__q_jE__) + ((((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 1.271 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * cos__q_jB__)) + ( 0.495 * cos__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + (((((- 0.443 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.443 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.443 * sin__q_jA__) * cos__q_jD__)) * sin__q_jF__) + ((((((((( 0.443 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.443 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.443 * sin__q_jA__) * sin__q_jD__) + ((((( 0.443 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.443 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(0,5) = ((((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * cos__q_jE__));
+ (*this)(1,0) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(1,1) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,2) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(1,3) = (((((((((( 0.443 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.443 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.443 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.443 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.443 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jF__) + ((((((((((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * cos__q_jC__)) - ( 0.175 * cos__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.443 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.443 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.443 * cos__q_jA__) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,4) = (((((((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * sin__q_jE__) + ((((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 1.271 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * cos__q_jB__)) + ( 0.495 * sin__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + (((((- 0.443 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.443 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.443 * cos__q_jA__) * cos__q_jD__)) * sin__q_jF__) + ((((((((( 0.443 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.443 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.443 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.443 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.443 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,5) = ((((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * cos__q_jE__));
+ (*this)(2,0) = ((((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + ((((( cos__q_jB__ * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(2,1) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(2,2) = ((((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(2,3) = ((((((((- 0.443 * cos__q_jB__) * sin__q_jC__) - (( 0.443 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.443 * cos__q_jB__) * cos__q_jC__) - (( 0.443 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__)) * sin__q_jF__) + (((((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.443 * cos__q_jB__) * cos__q_jC__) - (( 0.443 * sin__q_jB__) * sin__q_jC__)) * sin__q_jD__)) * cos__q_jF__));
+ (*this)(2,4) = ((((((((((( 1.271 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + (((- 0.175 * sin__q_jB__) - ( 1.271 * cos__q_jB__)) * cos__q_jC__)) - ( 1.095 * sin__q_jB__)) - 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.443 * sin__q_jB__) * sin__q_jC__) - (( 0.443 * cos__q_jB__) * cos__q_jC__)) * sin__q_jD__)) * sin__q_jF__) + (((((((- 0.443 * cos__q_jB__) * sin__q_jC__) - (( 0.443 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.443 * cos__q_jB__) * cos__q_jC__) - (( 0.443 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(2,5) = (((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * sin__q_jE__);
+ (*this)(3,3) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(3,4) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(3,5) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(4,3) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(4,4) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,5) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(5,3) = ((((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + ((((( cos__q_jB__ * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(5,4) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(5,5) = ((((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_com0::Type_fr_link0_X_com0()
+{
+ (*this)(0,0) = 1;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = - 0.075;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 1;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0.075;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = 1;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_com0& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_com0::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_com1::Type_fr_link0_X_com1()
+{
+ (*this)(0,2) = 0;
+ (*this)(1,2) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1.0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0.09;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_com1& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_com1::update(const JState& q) {
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+
+ (*this)(0,0) = cos__q_jA__;
+ (*this)(0,1) = - sin__q_jA__;
+ (*this)(0,3) = (- 0.25 * sin__q_jA__);
+ (*this)(0,4) = (- 0.25 * cos__q_jA__);
+ (*this)(0,5) = ( 0.09 * sin__q_jA__);
+ (*this)(1,0) = sin__q_jA__;
+ (*this)(1,1) = cos__q_jA__;
+ (*this)(1,3) = ( 0.25 * cos__q_jA__);
+ (*this)(1,4) = (- 0.25 * sin__q_jA__);
+ (*this)(1,5) = (- 0.09 * cos__q_jA__);
+ (*this)(3,3) = cos__q_jA__;
+ (*this)(3,4) = - sin__q_jA__;
+ (*this)(4,3) = sin__q_jA__;
+ (*this)(4,4) = cos__q_jA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_com2::Type_fr_link0_X_com2()
+{
+ (*this)(2,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_com2& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_com2::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+
+ (*this)(0,0) = ( cos__q_jA__ * sin__q_jB__);
+ (*this)(0,1) = ( cos__q_jA__ * cos__q_jB__);
+ (*this)(0,2) = - sin__q_jA__;
+ (*this)(0,3) = ((((- 0.495 * sin__q_jA__) * sin__q_jB__) + ((( 0.175 * sin__q_jA__) - ( 0.17 * cos__q_jA__)) * cos__q_jB__)) + ( 0.04 * sin__q_jA__));
+ (*this)(0,4) = ((((( 0.17 * cos__q_jA__) - ( 0.175 * sin__q_jA__)) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 0.47 * sin__q_jA__));
+ (*this)(0,5) = (((( 0.04 * cos__q_jA__) * sin__q_jB__) - (( 0.47 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__));
+ (*this)(1,0) = ( sin__q_jA__ * sin__q_jB__);
+ (*this)(1,1) = ( sin__q_jA__ * cos__q_jB__);
+ (*this)(1,2) = cos__q_jA__;
+ (*this)(1,3) = (((( 0.495 * cos__q_jA__) * sin__q_jB__) + (((- 0.17 * sin__q_jA__) - ( 0.175 * cos__q_jA__)) * cos__q_jB__)) - ( 0.04 * cos__q_jA__));
+ (*this)(1,4) = ((((( 0.17 * sin__q_jA__) + ( 0.175 * cos__q_jA__)) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 0.47 * cos__q_jA__));
+ (*this)(1,5) = (((( 0.04 * sin__q_jA__) * sin__q_jB__) - (( 0.47 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__));
+ (*this)(2,0) = cos__q_jB__;
+ (*this)(2,1) = - sin__q_jB__;
+ (*this)(2,3) = ( 0.17 * sin__q_jB__);
+ (*this)(2,4) = ( 0.17 * cos__q_jB__);
+ (*this)(2,5) = ((( 0.47 * sin__q_jB__) + ( 0.04 * cos__q_jB__)) + 0.175);
+ (*this)(3,3) = ( cos__q_jA__ * sin__q_jB__);
+ (*this)(3,4) = ( cos__q_jA__ * cos__q_jB__);
+ (*this)(3,5) = - sin__q_jA__;
+ (*this)(4,3) = ( sin__q_jA__ * sin__q_jB__);
+ (*this)(4,4) = ( sin__q_jA__ * cos__q_jB__);
+ (*this)(4,5) = cos__q_jA__;
+ (*this)(5,3) = cos__q_jB__;
+ (*this)(5,4) = - sin__q_jB__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_com3::Type_fr_link0_X_com3()
+{
+ (*this)(2,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_com3& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_com3::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+
+ (*this)(0,0) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(0,1) = (((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(0,2) = - sin__q_jA__;
+ (*this)(0,3) = (((((( 0.495 * sin__q_jA__) * sin__q_jB__) + (((- 0.175 * sin__q_jA__) - ( 0.044 * cos__q_jA__)) * cos__q_jB__)) * sin__q_jC__) + ((((((- 0.175 * sin__q_jA__) - ( 0.044 * cos__q_jA__)) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.133 * sin__q_jA__));
+ (*this)(0,4) = (((((((( 0.175 * sin__q_jA__) + ( 0.044 * cos__q_jA__)) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) + (((- 0.175 * sin__q_jA__) - ( 0.044 * cos__q_jA__)) * cos__q_jB__)) * cos__q_jC__)) - ( 0.035 * sin__q_jA__));
+ (*this)(0,5) = ((((((( 0.133 * cos__q_jA__) * sin__q_jB__) + (( 0.035 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.035 * cos__q_jA__) * sin__q_jB__) - (( 0.133 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__));
+ (*this)(1,0) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(1,1) = (((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(1,2) = cos__q_jA__;
+ (*this)(1,3) = ((((((( 0.175 * cos__q_jA__) - ( 0.044 * sin__q_jA__)) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + (((((( 0.175 * cos__q_jA__) - ( 0.044 * sin__q_jA__)) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.133 * cos__q_jA__));
+ (*this)(1,4) = (((((((( 0.044 * sin__q_jA__) - ( 0.175 * cos__q_jA__)) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) - ( 0.044 * sin__q_jA__)) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 0.035 * cos__q_jA__));
+ (*this)(1,5) = ((((((( 0.133 * sin__q_jA__) * sin__q_jB__) + (( 0.035 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.035 * sin__q_jA__) * sin__q_jB__) - (( 0.133 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__));
+ (*this)(2,0) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(2,1) = (( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__));
+ (*this)(2,3) = ((( 0.044 * sin__q_jB__) * sin__q_jC__) - (( 0.044 * cos__q_jB__) * cos__q_jC__));
+ (*this)(2,4) = ((( 0.044 * cos__q_jB__) * sin__q_jC__) + (( 0.044 * sin__q_jB__) * cos__q_jC__));
+ (*this)(2,5) = (((((( 0.133 * cos__q_jB__) - ( 0.035 * sin__q_jB__)) * sin__q_jC__) + ((( 0.133 * sin__q_jB__) + ( 0.035 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175);
+ (*this)(3,3) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(3,4) = (((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(3,5) = - sin__q_jA__;
+ (*this)(4,3) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(4,4) = (((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(4,5) = cos__q_jA__;
+ (*this)(5,3) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(5,4) = (( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__));
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_com4::Type_fr_link0_X_com4()
+{
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_com4& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_com4::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+
+ (*this)(0,0) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( sin__q_jA__ * cos__q_jD__));
+ (*this)(0,1) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__));
+ (*this)(0,2) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(0,3) = ((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 0.832 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.832 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.832 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * cos__q_jB__)) + ( 0.495 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(0,4) = ((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.832 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.832 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 0.832 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(0,5) = (((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__));
+ (*this)(1,0) = (((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( cos__q_jA__ * cos__q_jD__));
+ (*this)(1,1) = (( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__));
+ (*this)(1,2) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(1,3) = (((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 0.832 * cos__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.832 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.832 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * cos__q_jB__)) + ( 0.495 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(1,4) = ((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.832 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.832 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 0.832 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(1,5) = (((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__));
+ (*this)(2,0) = ((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * sin__q_jD__);
+ (*this)(2,1) = ((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__);
+ (*this)(2,2) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(2,3) = ((((((( 0.832 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + (((- 0.175 * sin__q_jB__) - ( 0.832 * cos__q_jB__)) * cos__q_jC__)) - ( 1.095 * sin__q_jB__)) - 0.175) * cos__q_jD__);
+ (*this)(2,4) = ((((((( 0.175 * cos__q_jB__) - ( 0.832 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 0.832 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__);
+ (*this)(3,3) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( sin__q_jA__ * cos__q_jD__));
+ (*this)(3,4) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__));
+ (*this)(3,5) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(4,3) = (((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( cos__q_jA__ * cos__q_jD__));
+ (*this)(4,4) = (( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__));
+ (*this)(4,5) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(5,3) = ((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * sin__q_jD__);
+ (*this)(5,4) = ((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__);
+ (*this)(5,5) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_com5::Type_fr_link0_X_com5()
+{
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_com5& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_com5::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+
+ (*this)(0,0) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(0,1) = ((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__));
+ (*this)(0,2) = (((((( 1.0 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(0,3) = ((((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * cos__q_jE__));
+ (*this)(0,4) = (((((((((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * cos__q_jC__)) + ( 0.175 * sin__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.07 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.07 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.07 * sin__q_jA__) * cos__q_jD__));
+ (*this)(0,5) = (((((((( 0.07 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.07 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.07 * sin__q_jA__) * sin__q_jD__) + ((((( 0.07 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.07 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(1,0) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(1,1) = ((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(1,2) = (((((( 1.0 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(1,3) = ((((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * cos__q_jE__));
+ (*this)(1,4) = (((((((((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * cos__q_jC__)) - ( 0.175 * cos__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.07 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.07 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.07 * cos__q_jA__) * cos__q_jD__));
+ (*this)(1,5) = (((((((( 0.07 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.07 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.07 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.07 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.07 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(2,0) = ((((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(2,1) = (((( cos__q_jB__ * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__));
+ (*this)(2,2) = (((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ (*this)(2,3) = (((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * sin__q_jE__);
+ (*this)(2,4) = ((((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.07 * cos__q_jB__) * cos__q_jC__) - (( 0.07 * sin__q_jB__) * sin__q_jC__)) * sin__q_jD__));
+ (*this)(2,5) = ((((((- 0.07 * cos__q_jB__) * sin__q_jC__) - (( 0.07 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.07 * cos__q_jB__) * cos__q_jC__) - (( 0.07 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__));
+ (*this)(3,3) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(3,4) = ((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__));
+ (*this)(3,5) = (((((( 1.0 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(4,3) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(4,4) = ((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(4,5) = (((((( 1.0 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(5,3) = ((((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(5,4) = (((( cos__q_jB__ * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__));
+ (*this)(5,5) = (((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_com6::Type_fr_link0_X_com6()
+{
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_com6& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_com6::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jF__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+ SCALAR cos__q_jF__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jF__ = TRAIT::sin( q(JF));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+ cos__q_jF__ = TRAIT::cos( q(JF));
+
+ (*this)(0,0) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,1) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(0,2) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(0,3) = (((((((((( 0.3429 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.3429 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.3429 * sin__q_jA__) * sin__q_jD__) + ((((( 0.3429 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.3429 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jF__) + ((((((((((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * cos__q_jC__)) + ( 0.175 * sin__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.3429 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.3429 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.3429 * sin__q_jA__) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(0,4) = (((((((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * sin__q_jE__) + ((((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 1.271 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * cos__q_jB__)) + ( 0.495 * cos__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + (((((- 0.3429 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.3429 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.3429 * sin__q_jA__) * cos__q_jD__)) * sin__q_jF__) + ((((((((( 0.3429 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.3429 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.3429 * sin__q_jA__) * sin__q_jD__) + ((((( 0.3429 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.3429 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(0,5) = ((((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * cos__q_jE__));
+ (*this)(1,0) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(1,1) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,2) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(1,3) = (((((((((( 0.3429 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.3429 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.3429 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.3429 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.3429 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jF__) + ((((((((((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * cos__q_jC__)) - ( 0.175 * cos__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.3429 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.3429 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.3429 * cos__q_jA__) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,4) = (((((((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * sin__q_jE__) + ((((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 1.271 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * cos__q_jB__)) + ( 0.495 * sin__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + (((((- 0.3429 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.3429 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.3429 * cos__q_jA__) * cos__q_jD__)) * sin__q_jF__) + ((((((((( 0.3429 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.3429 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.3429 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.3429 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.3429 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,5) = ((((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * cos__q_jE__));
+ (*this)(2,0) = ((((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + ((((( cos__q_jB__ * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(2,1) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(2,2) = ((((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(2,3) = ((((((((- 0.3429 * cos__q_jB__) * sin__q_jC__) - (( 0.3429 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.3429 * cos__q_jB__) * cos__q_jC__) - (( 0.3429 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__)) * sin__q_jF__) + (((((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.3429 * cos__q_jB__) * cos__q_jC__) - (( 0.3429 * sin__q_jB__) * sin__q_jC__)) * sin__q_jD__)) * cos__q_jF__));
+ (*this)(2,4) = ((((((((((( 1.271 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + (((- 0.175 * sin__q_jB__) - ( 1.271 * cos__q_jB__)) * cos__q_jC__)) - ( 1.095 * sin__q_jB__)) - 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.3429 * sin__q_jB__) * sin__q_jC__) - (( 0.3429 * cos__q_jB__) * cos__q_jC__)) * sin__q_jD__)) * sin__q_jF__) + (((((((- 0.3429 * cos__q_jB__) * sin__q_jC__) - (( 0.3429 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.3429 * cos__q_jB__) * cos__q_jC__) - (( 0.3429 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(2,5) = (((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * sin__q_jE__);
+ (*this)(3,3) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(3,4) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(3,5) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(4,3) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(4,4) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,5) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(5,3) = ((((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + ((((( cos__q_jB__ * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(5,4) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(5,5) = ((((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link1_X_fr_link0::Type_fr_link1_X_fr_link0()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link1_X_fr_link0& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link1_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+
+ (*this)(0,0) = cos__q_jA__;
+ (*this)(0,1) = sin__q_jA__;
+ (*this)(1,0) = - sin__q_jA__;
+ (*this)(1,1) = cos__q_jA__;
+ (*this)(3,3) = cos__q_jA__;
+ (*this)(3,4) = sin__q_jA__;
+ (*this)(4,3) = - sin__q_jA__;
+ (*this)(4,4) = cos__q_jA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link2_X_fr_link0::Type_fr_link2_X_fr_link0()
+{
+ (*this)(0,5) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,5) = 0.175;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link2_X_fr_link0& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link2_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+
+ (*this)(0,0) = ( cos__q_jA__ * sin__q_jB__);
+ (*this)(0,1) = ( sin__q_jA__ * sin__q_jB__);
+ (*this)(0,2) = cos__q_jB__;
+ (*this)(0,3) = ((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__));
+ (*this)(0,4) = ((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__));
+ (*this)(1,0) = ( cos__q_jA__ * cos__q_jB__);
+ (*this)(1,1) = ( sin__q_jA__ * cos__q_jB__);
+ (*this)(1,2) = - sin__q_jB__;
+ (*this)(1,3) = (((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__));
+ (*this)(1,4) = ((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__));
+ (*this)(2,0) = - sin__q_jA__;
+ (*this)(2,1) = cos__q_jA__;
+ (*this)(2,3) = (- 0.495 * cos__q_jA__);
+ (*this)(2,4) = (- 0.495 * sin__q_jA__);
+ (*this)(3,3) = ( cos__q_jA__ * sin__q_jB__);
+ (*this)(3,4) = ( sin__q_jA__ * sin__q_jB__);
+ (*this)(3,5) = cos__q_jB__;
+ (*this)(4,3) = ( cos__q_jA__ * cos__q_jB__);
+ (*this)(4,4) = ( sin__q_jA__ * cos__q_jB__);
+ (*this)(4,5) = - sin__q_jB__;
+ (*this)(5,3) = - sin__q_jA__;
+ (*this)(5,4) = cos__q_jA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link3_X_fr_link0::Type_fr_link3_X_fr_link0()
+{
+ (*this)(0,5) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link3_X_fr_link0& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link3_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+
+ (*this)(0,0) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(0,1) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(0,2) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(0,3) = ((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__));
+ (*this)(0,4) = ((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__));
+ (*this)(1,0) = (((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(1,1) = (((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(1,2) = (( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__));
+ (*this)(1,3) = (((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__));
+ (*this)(1,4) = ((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__));
+ (*this)(2,0) = - sin__q_jA__;
+ (*this)(2,1) = cos__q_jA__;
+ (*this)(2,3) = (((- 1.095 * cos__q_jA__) * cos__q_jB__) - ( 0.495 * cos__q_jA__));
+ (*this)(2,4) = (((- 1.095 * sin__q_jA__) * cos__q_jB__) - ( 0.495 * sin__q_jA__));
+ (*this)(2,5) = (( 1.095 * sin__q_jB__) + 0.175);
+ (*this)(3,3) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(3,4) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(3,5) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(4,3) = (((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(4,4) = (((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(4,5) = (( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__));
+ (*this)(5,3) = - sin__q_jA__;
+ (*this)(5,4) = cos__q_jA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link4_X_fr_link0::Type_fr_link4_X_fr_link0()
+{
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link4_X_fr_link0& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link4_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+
+ (*this)(0,0) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( sin__q_jA__ * cos__q_jD__));
+ (*this)(0,1) = (((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( cos__q_jA__ * cos__q_jD__));
+ (*this)(0,2) = (((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * sin__q_jD__);
+ (*this)(0,3) = ((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 0.332 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.332 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.332 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * cos__q_jB__)) + ( 0.495 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(0,4) = (((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 0.332 * cos__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.332 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.332 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * cos__q_jB__)) + ( 0.495 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(0,5) = ((((((( 0.332 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + (((- 0.175 * sin__q_jB__) - ( 0.332 * cos__q_jB__)) * cos__q_jC__)) - ( 1.095 * sin__q_jB__)) - 0.175) * cos__q_jD__);
+ (*this)(1,0) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__));
+ (*this)(1,1) = (( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__));
+ (*this)(1,2) = (((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__);
+ (*this)(1,3) = ((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.332 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.332 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 0.332 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(1,4) = ((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.332 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.332 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 0.332 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(1,5) = ((((((( 0.175 * cos__q_jB__) - ( 0.332 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 0.332 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__);
+ (*this)(2,0) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(2,1) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(2,2) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(2,3) = (((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__));
+ (*this)(2,4) = (((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__));
+ (*this)(3,3) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( sin__q_jA__ * cos__q_jD__));
+ (*this)(3,4) = (((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( cos__q_jA__ * cos__q_jD__));
+ (*this)(3,5) = (((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * sin__q_jD__);
+ (*this)(4,3) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__));
+ (*this)(4,4) = (( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__));
+ (*this)(4,5) = (((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__);
+ (*this)(5,3) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(5,4) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(5,5) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link5_X_fr_link0::Type_fr_link5_X_fr_link0()
+{
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link5_X_fr_link0& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link5_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+
+ (*this)(0,0) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(0,1) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(0,2) = (((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(0,3) = ((((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * cos__q_jE__));
+ (*this)(0,4) = ((((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * cos__q_jE__));
+ (*this)(0,5) = (((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * sin__q_jE__);
+ (*this)(1,0) = ((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__));
+ (*this)(1,1) = ((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(1,2) = ((((( 1.0 * cos__q_jB__) * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__));
+ (*this)(1,3) = (((((((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * cos__q_jC__)) + ( 0.175 * sin__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(1,4) = (((((((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * cos__q_jC__)) - ( 0.175 * cos__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(1,5) = (((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * cos__q_jE__);
+ (*this)(2,0) = ((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(2,1) = ((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(2,2) = (((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ (*this)(2,3) = (((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(2,4) = ((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(2,5) = ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__);
+ (*this)(3,3) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(3,4) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(3,5) = (((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(4,3) = ((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__));
+ (*this)(4,4) = ((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(4,5) = ((((( 1.0 * cos__q_jB__) * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__));
+ (*this)(5,3) = ((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(5,4) = ((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(5,5) = (((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link6_X_fr_link0::Type_fr_link6_X_fr_link0()
+{
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link6_X_fr_link0& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link6_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jF__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+ SCALAR cos__q_jF__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jF__ = TRAIT::sin( q(JF));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+ cos__q_jF__ = TRAIT::cos( q(JF));
+
+ (*this)(0,0) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,1) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,2) = (((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + (((((( 1.0 * cos__q_jB__) * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,3) = (((((((((( 0.243 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.243 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.243 * sin__q_jA__) * sin__q_jD__) + ((((( 0.243 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jF__) + ((((((((((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * cos__q_jC__)) + ( 0.175 * sin__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.243 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.243 * sin__q_jA__) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(0,4) = (((((((((( 0.243 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.243 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.243 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.243 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jF__) + ((((((((((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * cos__q_jC__)) - ( 0.175 * cos__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.243 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.243 * cos__q_jA__) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(0,5) = ((((((((- 0.243 * cos__q_jB__) * sin__q_jC__) - (( 0.243 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.243 * cos__q_jB__) * cos__q_jC__) - (( 0.243 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__)) * sin__q_jF__) + (((((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.243 * cos__q_jB__) * cos__q_jC__) - (( 0.243 * sin__q_jB__) * sin__q_jC__)) * sin__q_jD__)) * cos__q_jF__));
+ (*this)(1,0) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,1) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,2) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(1,3) = (((((((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * sin__q_jE__) + ((((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 1.271 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * cos__q_jB__)) + ( 0.495 * cos__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + (((((- 0.243 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.243 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.243 * sin__q_jA__) * cos__q_jD__)) * sin__q_jF__) + ((((((((( 0.243 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.243 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.243 * sin__q_jA__) * sin__q_jD__) + ((((( 0.243 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,4) = (((((((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * sin__q_jE__) + ((((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 1.271 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * cos__q_jB__)) + ( 0.495 * sin__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + (((((- 0.243 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.243 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.243 * cos__q_jA__) * cos__q_jD__)) * sin__q_jF__) + ((((((((( 0.243 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.243 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.243 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.243 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,5) = ((((((((((( 1.271 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + (((- 0.175 * sin__q_jB__) - ( 1.271 * cos__q_jB__)) * cos__q_jC__)) - ( 1.095 * sin__q_jB__)) - 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.243 * sin__q_jB__) * sin__q_jC__) - (( 0.243 * cos__q_jB__) * cos__q_jC__)) * sin__q_jD__)) * sin__q_jF__) + (((((((- 0.243 * cos__q_jB__) * sin__q_jC__) - (( 0.243 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.243 * cos__q_jB__) * cos__q_jC__) - (( 0.243 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(2,0) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(2,1) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(2,2) = (((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(2,3) = ((((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * cos__q_jE__));
+ (*this)(2,4) = ((((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * cos__q_jE__));
+ (*this)(2,5) = (((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * sin__q_jE__);
+ (*this)(3,3) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(3,4) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(3,5) = (((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + (((((( 1.0 * cos__q_jB__) * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(4,3) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,4) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,5) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(5,3) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(5,4) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(5,5) = (((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_ee_X_fr_link0::Type_fr_ee_X_fr_link0()
+{
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_ee_X_fr_link0& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_ee_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jF__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+ SCALAR cos__q_jF__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jF__ = TRAIT::sin( q(JF));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+ cos__q_jF__ = TRAIT::cos( q(JF));
+
+ (*this)(0,0) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,1) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,2) = (((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + (((((( 1.0 * cos__q_jB__) * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,3) = (((((((((( 0.443 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.443 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.443 * sin__q_jA__) * sin__q_jD__) + ((((( 0.443 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.443 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jF__) + ((((((((((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * cos__q_jC__)) + ( 0.175 * sin__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.443 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.443 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.443 * sin__q_jA__) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(0,4) = (((((((((( 0.443 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.443 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.443 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.443 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.443 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jF__) + ((((((((((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * cos__q_jC__)) - ( 0.175 * cos__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.443 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.443 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.443 * cos__q_jA__) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(0,5) = ((((((((- 0.443 * cos__q_jB__) * sin__q_jC__) - (( 0.443 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.443 * cos__q_jB__) * cos__q_jC__) - (( 0.443 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__)) * sin__q_jF__) + (((((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.443 * cos__q_jB__) * cos__q_jC__) - (( 0.443 * sin__q_jB__) * sin__q_jC__)) * sin__q_jD__)) * cos__q_jF__));
+ (*this)(1,0) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,1) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,2) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(1,3) = (((((((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * sin__q_jE__) + ((((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 1.271 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * cos__q_jB__)) + ( 0.495 * cos__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + (((((- 0.443 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.443 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.443 * sin__q_jA__) * cos__q_jD__)) * sin__q_jF__) + ((((((((( 0.443 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.443 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.443 * sin__q_jA__) * sin__q_jD__) + ((((( 0.443 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.443 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,4) = (((((((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * sin__q_jE__) + ((((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 1.271 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * cos__q_jB__)) + ( 0.495 * sin__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + (((((- 0.443 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.443 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.443 * cos__q_jA__) * cos__q_jD__)) * sin__q_jF__) + ((((((((( 0.443 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.443 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.443 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.443 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.443 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,5) = ((((((((((( 1.271 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + (((- 0.175 * sin__q_jB__) - ( 1.271 * cos__q_jB__)) * cos__q_jC__)) - ( 1.095 * sin__q_jB__)) - 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.443 * sin__q_jB__) * sin__q_jC__) - (( 0.443 * cos__q_jB__) * cos__q_jC__)) * sin__q_jD__)) * sin__q_jF__) + (((((((- 0.443 * cos__q_jB__) * sin__q_jC__) - (( 0.443 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.443 * cos__q_jB__) * cos__q_jC__) - (( 0.443 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(2,0) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(2,1) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(2,2) = (((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(2,3) = ((((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * cos__q_jE__));
+ (*this)(2,4) = ((((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * cos__q_jE__));
+ (*this)(2,5) = (((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * sin__q_jE__);
+ (*this)(3,3) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(3,4) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(3,5) = (((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + (((((( 1.0 * cos__q_jB__) * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(4,3) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,4) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,5) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(5,3) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(5,4) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(5,5) = (((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_com0_X_fr_link0::Type_com0_X_fr_link0()
+{
+ (*this)(0,0) = 1;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0.075;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 1;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = - 0.075;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = 1;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_com0_X_fr_link0& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_com0_X_fr_link0::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_com1_X_fr_link0::Type_com1_X_fr_link0()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,5) = 0.09;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1.0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_com1_X_fr_link0& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_com1_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+
+ (*this)(0,0) = cos__q_jA__;
+ (*this)(0,1) = sin__q_jA__;
+ (*this)(0,3) = (- 0.25 * sin__q_jA__);
+ (*this)(0,4) = ( 0.25 * cos__q_jA__);
+ (*this)(1,0) = - sin__q_jA__;
+ (*this)(1,1) = cos__q_jA__;
+ (*this)(1,3) = (- 0.25 * cos__q_jA__);
+ (*this)(1,4) = (- 0.25 * sin__q_jA__);
+ (*this)(2,3) = ( 0.09 * sin__q_jA__);
+ (*this)(2,4) = (- 0.09 * cos__q_jA__);
+ (*this)(3,3) = cos__q_jA__;
+ (*this)(3,4) = sin__q_jA__;
+ (*this)(4,3) = - sin__q_jA__;
+ (*this)(4,4) = cos__q_jA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_com2_X_fr_link0::Type_com2_X_fr_link0()
+{
+ (*this)(2,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_com2_X_fr_link0& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_com2_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+
+ (*this)(0,0) = ( cos__q_jA__ * sin__q_jB__);
+ (*this)(0,1) = ( sin__q_jA__ * sin__q_jB__);
+ (*this)(0,2) = cos__q_jB__;
+ (*this)(0,3) = ((((- 0.495 * sin__q_jA__) * sin__q_jB__) + ((( 0.175 * sin__q_jA__) - ( 0.17 * cos__q_jA__)) * cos__q_jB__)) + ( 0.04 * sin__q_jA__));
+ (*this)(0,4) = (((( 0.495 * cos__q_jA__) * sin__q_jB__) + (((- 0.17 * sin__q_jA__) - ( 0.175 * cos__q_jA__)) * cos__q_jB__)) - ( 0.04 * cos__q_jA__));
+ (*this)(0,5) = ( 0.17 * sin__q_jB__);
+ (*this)(1,0) = ( cos__q_jA__ * cos__q_jB__);
+ (*this)(1,1) = ( sin__q_jA__ * cos__q_jB__);
+ (*this)(1,2) = - sin__q_jB__;
+ (*this)(1,3) = ((((( 0.17 * cos__q_jA__) - ( 0.175 * sin__q_jA__)) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 0.47 * sin__q_jA__));
+ (*this)(1,4) = ((((( 0.17 * sin__q_jA__) + ( 0.175 * cos__q_jA__)) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 0.47 * cos__q_jA__));
+ (*this)(1,5) = ( 0.17 * cos__q_jB__);
+ (*this)(2,0) = - sin__q_jA__;
+ (*this)(2,1) = cos__q_jA__;
+ (*this)(2,3) = (((( 0.04 * cos__q_jA__) * sin__q_jB__) - (( 0.47 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__));
+ (*this)(2,4) = (((( 0.04 * sin__q_jA__) * sin__q_jB__) - (( 0.47 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__));
+ (*this)(2,5) = ((( 0.47 * sin__q_jB__) + ( 0.04 * cos__q_jB__)) + 0.175);
+ (*this)(3,3) = ( cos__q_jA__ * sin__q_jB__);
+ (*this)(3,4) = ( sin__q_jA__ * sin__q_jB__);
+ (*this)(3,5) = cos__q_jB__;
+ (*this)(4,3) = ( cos__q_jA__ * cos__q_jB__);
+ (*this)(4,4) = ( sin__q_jA__ * cos__q_jB__);
+ (*this)(4,5) = - sin__q_jB__;
+ (*this)(5,3) = - sin__q_jA__;
+ (*this)(5,4) = cos__q_jA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_com3_X_fr_link0::Type_com3_X_fr_link0()
+{
+ (*this)(2,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_com3_X_fr_link0& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_com3_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+
+ (*this)(0,0) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(0,1) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(0,2) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(0,3) = (((((( 0.495 * sin__q_jA__) * sin__q_jB__) + (((- 0.175 * sin__q_jA__) - ( 0.044 * cos__q_jA__)) * cos__q_jB__)) * sin__q_jC__) + ((((((- 0.175 * sin__q_jA__) - ( 0.044 * cos__q_jA__)) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.133 * sin__q_jA__));
+ (*this)(0,4) = ((((((( 0.175 * cos__q_jA__) - ( 0.044 * sin__q_jA__)) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + (((((( 0.175 * cos__q_jA__) - ( 0.044 * sin__q_jA__)) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.133 * cos__q_jA__));
+ (*this)(0,5) = ((( 0.044 * sin__q_jB__) * sin__q_jC__) - (( 0.044 * cos__q_jB__) * cos__q_jC__));
+ (*this)(1,0) = (((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(1,1) = (((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(1,2) = (( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__));
+ (*this)(1,3) = (((((((( 0.175 * sin__q_jA__) + ( 0.044 * cos__q_jA__)) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) + (((- 0.175 * sin__q_jA__) - ( 0.044 * cos__q_jA__)) * cos__q_jB__)) * cos__q_jC__)) - ( 0.035 * sin__q_jA__));
+ (*this)(1,4) = (((((((( 0.044 * sin__q_jA__) - ( 0.175 * cos__q_jA__)) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) - ( 0.044 * sin__q_jA__)) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 0.035 * cos__q_jA__));
+ (*this)(1,5) = ((( 0.044 * cos__q_jB__) * sin__q_jC__) + (( 0.044 * sin__q_jB__) * cos__q_jC__));
+ (*this)(2,0) = - sin__q_jA__;
+ (*this)(2,1) = cos__q_jA__;
+ (*this)(2,3) = ((((((( 0.133 * cos__q_jA__) * sin__q_jB__) + (( 0.035 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.035 * cos__q_jA__) * sin__q_jB__) - (( 0.133 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__));
+ (*this)(2,4) = ((((((( 0.133 * sin__q_jA__) * sin__q_jB__) + (( 0.035 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.035 * sin__q_jA__) * sin__q_jB__) - (( 0.133 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__));
+ (*this)(2,5) = (((((( 0.133 * cos__q_jB__) - ( 0.035 * sin__q_jB__)) * sin__q_jC__) + ((( 0.133 * sin__q_jB__) + ( 0.035 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175);
+ (*this)(3,3) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(3,4) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(3,5) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(4,3) = (((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(4,4) = (((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(4,5) = (( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__));
+ (*this)(5,3) = - sin__q_jA__;
+ (*this)(5,4) = cos__q_jA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_com4_X_fr_link0::Type_com4_X_fr_link0()
+{
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_com4_X_fr_link0& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_com4_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+
+ (*this)(0,0) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( sin__q_jA__ * cos__q_jD__));
+ (*this)(0,1) = (((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( cos__q_jA__ * cos__q_jD__));
+ (*this)(0,2) = (((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * sin__q_jD__);
+ (*this)(0,3) = ((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 0.832 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.832 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.832 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * cos__q_jB__)) + ( 0.495 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(0,4) = (((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 0.832 * cos__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.832 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.832 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * cos__q_jB__)) + ( 0.495 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(0,5) = ((((((( 0.832 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + (((- 0.175 * sin__q_jB__) - ( 0.832 * cos__q_jB__)) * cos__q_jC__)) - ( 1.095 * sin__q_jB__)) - 0.175) * cos__q_jD__);
+ (*this)(1,0) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__));
+ (*this)(1,1) = (( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__));
+ (*this)(1,2) = (((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__);
+ (*this)(1,3) = ((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.832 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.832 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 0.832 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(1,4) = ((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.832 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.832 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 0.832 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(1,5) = ((((((( 0.175 * cos__q_jB__) - ( 0.832 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 0.832 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__);
+ (*this)(2,0) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(2,1) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(2,2) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(2,3) = (((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__));
+ (*this)(2,4) = (((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__));
+ (*this)(3,3) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( sin__q_jA__ * cos__q_jD__));
+ (*this)(3,4) = (((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( cos__q_jA__ * cos__q_jD__));
+ (*this)(3,5) = (((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * sin__q_jD__);
+ (*this)(4,3) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__));
+ (*this)(4,4) = (( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__));
+ (*this)(4,5) = (((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__);
+ (*this)(5,3) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(5,4) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(5,5) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_com5_X_fr_link0::Type_com5_X_fr_link0()
+{
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_com5_X_fr_link0& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_com5_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+
+ (*this)(0,0) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(0,1) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(0,2) = (((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(0,3) = ((((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * cos__q_jE__));
+ (*this)(0,4) = ((((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * cos__q_jE__));
+ (*this)(0,5) = (((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * sin__q_jE__);
+ (*this)(1,0) = ((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__));
+ (*this)(1,1) = ((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(1,2) = ((((( 1.0 * cos__q_jB__) * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__));
+ (*this)(1,3) = (((((((((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * cos__q_jC__)) + ( 0.175 * sin__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.07 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.07 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.07 * sin__q_jA__) * cos__q_jD__));
+ (*this)(1,4) = (((((((((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * cos__q_jC__)) - ( 0.175 * cos__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.07 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.07 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.07 * cos__q_jA__) * cos__q_jD__));
+ (*this)(1,5) = ((((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.07 * cos__q_jB__) * cos__q_jC__) - (( 0.07 * sin__q_jB__) * sin__q_jC__)) * sin__q_jD__));
+ (*this)(2,0) = ((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(2,1) = ((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(2,2) = (((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ (*this)(2,3) = (((((((( 0.07 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.07 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.07 * sin__q_jA__) * sin__q_jD__) + ((((( 0.07 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.07 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(2,4) = (((((((( 0.07 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.07 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.07 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.07 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.07 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(2,5) = ((((((- 0.07 * cos__q_jB__) * sin__q_jC__) - (( 0.07 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.07 * cos__q_jB__) * cos__q_jC__) - (( 0.07 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__));
+ (*this)(3,3) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(3,4) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(3,5) = (((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(4,3) = ((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__));
+ (*this)(4,4) = ((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(4,5) = ((((( 1.0 * cos__q_jB__) * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__));
+ (*this)(5,3) = ((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(5,4) = ((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(5,5) = (((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_com6_X_fr_link0::Type_com6_X_fr_link0()
+{
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_com6_X_fr_link0& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_com6_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jF__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+ SCALAR cos__q_jF__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jF__ = TRAIT::sin( q(JF));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+ cos__q_jF__ = TRAIT::cos( q(JF));
+
+ (*this)(0,0) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,1) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,2) = (((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + (((((( 1.0 * cos__q_jB__) * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,3) = (((((((((( 0.3429 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.3429 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.3429 * sin__q_jA__) * sin__q_jD__) + ((((( 0.3429 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.3429 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jF__) + ((((((((((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * cos__q_jC__)) + ( 0.175 * sin__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.3429 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.3429 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.3429 * sin__q_jA__) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(0,4) = (((((((((( 0.3429 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.3429 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.3429 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.3429 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.3429 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jF__) + ((((((((((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * cos__q_jC__)) - ( 0.175 * cos__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.3429 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.3429 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.3429 * cos__q_jA__) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(0,5) = ((((((((- 0.3429 * cos__q_jB__) * sin__q_jC__) - (( 0.3429 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.3429 * cos__q_jB__) * cos__q_jC__) - (( 0.3429 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__)) * sin__q_jF__) + (((((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.3429 * cos__q_jB__) * cos__q_jC__) - (( 0.3429 * sin__q_jB__) * sin__q_jC__)) * sin__q_jD__)) * cos__q_jF__));
+ (*this)(1,0) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,1) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,2) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(1,3) = (((((((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * sin__q_jE__) + ((((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 1.271 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * cos__q_jB__)) + ( 0.495 * cos__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + (((((- 0.3429 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.3429 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.3429 * sin__q_jA__) * cos__q_jD__)) * sin__q_jF__) + ((((((((( 0.3429 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.3429 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.3429 * sin__q_jA__) * sin__q_jD__) + ((((( 0.3429 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.3429 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,4) = (((((((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * sin__q_jE__) + ((((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 1.271 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * cos__q_jB__)) + ( 0.495 * sin__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + (((((- 0.3429 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.3429 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.3429 * cos__q_jA__) * cos__q_jD__)) * sin__q_jF__) + ((((((((( 0.3429 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.3429 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.3429 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.3429 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.3429 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,5) = ((((((((((( 1.271 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + (((- 0.175 * sin__q_jB__) - ( 1.271 * cos__q_jB__)) * cos__q_jC__)) - ( 1.095 * sin__q_jB__)) - 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.3429 * sin__q_jB__) * sin__q_jC__) - (( 0.3429 * cos__q_jB__) * cos__q_jC__)) * sin__q_jD__)) * sin__q_jF__) + (((((((- 0.3429 * cos__q_jB__) * sin__q_jC__) - (( 0.3429 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.3429 * cos__q_jB__) * cos__q_jC__) - (( 0.3429 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__)) * cos__q_jF__));
+ (*this)(2,0) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(2,1) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(2,2) = (((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(2,3) = ((((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * cos__q_jE__));
+ (*this)(2,4) = ((((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * cos__q_jE__));
+ (*this)(2,5) = (((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * sin__q_jE__);
+ (*this)(3,3) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(3,4) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(3,5) = (((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + (((((( 1.0 * cos__q_jB__) * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(4,3) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,4) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(4,5) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(5,3) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(5,4) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(5,5) = (((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_jA::Type_fr_link0_X_fr_jA()
+{
+ (*this)(0,0) = 1;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 1.0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1.0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = 1.0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_jA& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_jA::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_jB::Type_fr_link0_X_fr_jB()
+{
+ (*this)(0,0) = 0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0.175;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 1.0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_jB& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_jB::update(const JState& q) {
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+
+ (*this)(0,1) = cos__q_jA__;
+ (*this)(0,2) = - sin__q_jA__;
+ (*this)(0,3) = ( 0.175 * sin__q_jA__);
+ (*this)(0,4) = (- 0.495 * sin__q_jA__);
+ (*this)(0,5) = (- 0.495 * cos__q_jA__);
+ (*this)(1,1) = sin__q_jA__;
+ (*this)(1,2) = cos__q_jA__;
+ (*this)(1,3) = (- 0.175 * cos__q_jA__);
+ (*this)(1,4) = ( 0.495 * cos__q_jA__);
+ (*this)(1,5) = (- 0.495 * sin__q_jA__);
+ (*this)(3,4) = cos__q_jA__;
+ (*this)(3,5) = - sin__q_jA__;
+ (*this)(4,4) = sin__q_jA__;
+ (*this)(4,5) = cos__q_jA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_jC::Type_fr_link0_X_fr_jC()
+{
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_jC& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_jC::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+
+ (*this)(0,0) = ( cos__q_jA__ * cos__q_jB__);
+ (*this)(0,1) = (- cos__q_jA__ * sin__q_jB__);
+ (*this)(0,2) = - sin__q_jA__;
+ (*this)(0,3) = ((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__));
+ (*this)(0,4) = ((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__));
+ (*this)(0,5) = (((- 1.095 * cos__q_jA__) * cos__q_jB__) - ( 0.495 * cos__q_jA__));
+ (*this)(1,0) = ( sin__q_jA__ * cos__q_jB__);
+ (*this)(1,1) = (- sin__q_jA__ * sin__q_jB__);
+ (*this)(1,2) = cos__q_jA__;
+ (*this)(1,3) = (((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__));
+ (*this)(1,4) = ((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__));
+ (*this)(1,5) = (((- 1.095 * sin__q_jA__) * cos__q_jB__) - ( 0.495 * sin__q_jA__));
+ (*this)(2,0) = - sin__q_jB__;
+ (*this)(2,1) = - cos__q_jB__;
+ (*this)(2,5) = (( 1.095 * sin__q_jB__) + 0.175);
+ (*this)(3,3) = ( cos__q_jA__ * cos__q_jB__);
+ (*this)(3,4) = (- cos__q_jA__ * sin__q_jB__);
+ (*this)(3,5) = - sin__q_jA__;
+ (*this)(4,3) = ( sin__q_jA__ * cos__q_jB__);
+ (*this)(4,4) = (- sin__q_jA__ * sin__q_jB__);
+ (*this)(4,5) = cos__q_jA__;
+ (*this)(5,3) = - sin__q_jB__;
+ (*this)(5,4) = - cos__q_jB__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_jD::Type_fr_link0_X_fr_jD()
+{
+ (*this)(2,0) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_jD& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_jD::update(const JState& q) {
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+
+ (*this)(0,0) = sin__q_jA__;
+ (*this)(0,1) = (((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(0,2) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(0,3) = (((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.332 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.332 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * cos__q_jB__)) + ( 0.495 * cos__q_jA__));
+ (*this)(0,4) = ((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 0.332 * sin__q_jA__));
+ (*this)(0,5) = (((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__));
+ (*this)(1,0) = - cos__q_jA__;
+ (*this)(1,1) = (((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(1,2) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(1,3) = (((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.332 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.332 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * cos__q_jB__)) + ( 0.495 * sin__q_jA__));
+ (*this)(1,4) = (((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 0.332 * cos__q_jA__));
+ (*this)(1,5) = (((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__));
+ (*this)(2,1) = (( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__));
+ (*this)(2,2) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(2,3) = (((((( 0.332 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + (((- 0.175 * sin__q_jB__) - ( 0.332 * cos__q_jB__)) * cos__q_jC__)) - ( 1.095 * sin__q_jB__)) - 0.175);
+ (*this)(3,3) = sin__q_jA__;
+ (*this)(3,4) = (((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(3,5) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(4,3) = - cos__q_jA__;
+ (*this)(4,4) = (((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(4,5) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(5,4) = (( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__));
+ (*this)(5,5) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_jE::Type_fr_link0_X_fr_jE()
+{
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_jE& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_jE::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jD__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+
+ (*this)(0,0) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(0,1) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__));
+ (*this)(0,2) = (((((( 1.0 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(0,3) = (((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__));
+ (*this)(0,4) = ((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(0,5) = (((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(1,0) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(1,1) = (( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__));
+ (*this)(1,2) = (((((( 1.0 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(1,3) = (((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__));
+ (*this)(1,4) = ((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(1,5) = ((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(2,0) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(2,1) = (((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__);
+ (*this)(2,2) = (((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ (*this)(2,4) = ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__);
+ (*this)(2,5) = ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__);
+ (*this)(3,3) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(3,4) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__));
+ (*this)(3,5) = ((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(4,3) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(4,4) = (( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__));
+ (*this)(4,5) = ((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(5,3) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(5,4) = ((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__);
+ (*this)(5,5) = ((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_jF::Type_fr_link0_X_fr_jF()
+{
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_jF& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link0_X_fr_jF::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jE__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jD__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+
+ (*this)(0,0) = (((((( 1.0 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__));
+ (*this)(0,1) = (((((( 1.0 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(0,2) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(0,3) = (((((((((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * cos__q_jC__)) + ( 0.175 * sin__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.243 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) - (( 0.243 * sin__q_jA__) * cos__q_jD__));
+ (*this)(0,4) = (((((((( 0.243 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.243 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((( 0.243 * sin__q_jA__) * sin__q_jD__) + ((((( 0.243 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) + ((((((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.495 * sin__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * sin__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * cos__q_jD__));
+ (*this)(0,5) = ((((((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * cos__q_jA__) * cos__q_jB__)) - ( 0.495 * cos__q_jA__)) * sin__q_jD__) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.495 * sin__q_jA__) * cos__q_jB__)) + ( 1.095 * sin__q_jA__)) * sin__q_jC__) + (((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * sin__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.495 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * sin__q_jA__) * sin__q_jB__) - (( 0.495 * sin__q_jA__) * cos__q_jB__)) - ( 1.095 * sin__q_jA__)) * cos__q_jC__)) - ( 0.175 * sin__q_jA__)) * cos__q_jE__));
+ (*this)(1,0) = (((((( 1.0 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + (((( 1.0 * cos__q_jA__) * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(1,1) = (((((( 1.0 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(1,2) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(1,3) = (((((((((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * cos__q_jC__)) - ( 0.175 * cos__q_jA__)) * sin__q_jE__) + (((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * cos__q_jE__)) + ((((( 0.243 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__)) + (( 0.243 * cos__q_jA__) * cos__q_jD__));
+ (*this)(1,4) = (((((((( 0.243 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.243 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((((((( 0.243 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + ((( 0.243 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.243 * cos__q_jA__) * sin__q_jD__)) * cos__q_jE__)) + (((((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.495 * cos__q_jA__) * sin__q_jB__) - (( 0.175 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - ( 1.271 * cos__q_jA__)) * sin__q_jD__)) + (((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * cos__q_jD__));
+ (*this)(1,5) = ((((((((((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * sin__q_jC__) + (((( 1.271 * sin__q_jA__) * sin__q_jB__) - (( 0.175 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) - (( 1.095 * sin__q_jA__) * cos__q_jB__)) - ( 0.495 * sin__q_jA__)) * sin__q_jD__) + ((((((((- 0.175 * cos__q_jA__) * sin__q_jB__) - (( 0.495 * cos__q_jA__) * cos__q_jB__)) - ( 1.095 * cos__q_jA__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * cos__q_jC__)) + ( 1.271 * cos__q_jA__)) * cos__q_jD__)) * sin__q_jE__) + ((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.495 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + ((((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.495 * cos__q_jA__) * cos__q_jB__)) + ( 1.095 * cos__q_jA__)) * cos__q_jC__)) + ( 0.175 * cos__q_jA__)) * cos__q_jE__));
+ (*this)(2,0) = ((((( 1.0 * cos__q_jB__) * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__));
+ (*this)(2,1) = (((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ (*this)(2,2) = ((((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(2,3) = ((((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * cos__q_jE__) + (((( 0.243 * cos__q_jB__) * cos__q_jC__) - (( 0.243 * sin__q_jB__) * sin__q_jC__)) * sin__q_jD__));
+ (*this)(2,4) = ((((((- 0.243 * cos__q_jB__) * sin__q_jC__) - (( 0.243 * sin__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((( 0.243 * cos__q_jB__) * cos__q_jC__) - (( 0.243 * sin__q_jB__) * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) + ((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * cos__q_jD__));
+ (*this)(2,5) = (((((((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * sin__q_jC__) + ((( 0.175 * sin__q_jB__) + ( 1.271 * cos__q_jB__)) * cos__q_jC__)) + ( 1.095 * sin__q_jB__)) + 0.175) * sin__q_jD__) * sin__q_jE__);
+ (*this)(3,3) = ((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__));
+ (*this)(3,4) = (((((( 1.0 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(3,5) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(4,3) = ((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(4,4) = (((((( 1.0 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(4,5) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(5,3) = (((( cos__q_jB__ * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__));
+ (*this)(5,4) = (((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ (*this)(5,5) = ((((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link2_X_fr_link1::Type_fr_link2_X_fr_link1()
+{
+ (*this)(0,1) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,1) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 1.0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = - 0.495;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0.175;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,4) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,4) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 1.0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link2_X_fr_link1& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link2_X_fr_link1::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR cos__q_jB__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+
+ (*this)(0,0) = sin__q_jB__;
+ (*this)(0,2) = cos__q_jB__;
+ (*this)(0,4) = (( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__));
+ (*this)(1,0) = cos__q_jB__;
+ (*this)(1,2) = - sin__q_jB__;
+ (*this)(1,4) = (( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__));
+ (*this)(3,3) = sin__q_jB__;
+ (*this)(3,5) = cos__q_jB__;
+ (*this)(4,3) = cos__q_jB__;
+ (*this)(4,5) = - sin__q_jB__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link1_X_fr_link2::Type_fr_link1_X_fr_link2()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = - 0.495;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 0;
+ (*this)(1,2) = 1.0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0.175;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = 0;
+ (*this)(4,5) = 1.0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link1_X_fr_link2& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link1_X_fr_link2::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR cos__q_jB__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+
+ (*this)(0,0) = sin__q_jB__;
+ (*this)(0,1) = cos__q_jB__;
+ (*this)(1,3) = (( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__));
+ (*this)(1,4) = (( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__));
+ (*this)(2,0) = cos__q_jB__;
+ (*this)(2,1) = - sin__q_jB__;
+ (*this)(3,3) = sin__q_jB__;
+ (*this)(3,4) = cos__q_jB__;
+ (*this)(5,3) = cos__q_jB__;
+ (*this)(5,4) = - sin__q_jB__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link3_X_fr_link2::Type_fr_link3_X_fr_link2()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = - 1.095;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link3_X_fr_link2& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link3_X_fr_link2::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR cos__q_jC__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+
+ (*this)(0,0) = - sin__q_jC__;
+ (*this)(0,1) = cos__q_jC__;
+ (*this)(0,5) = ( 1.095 * cos__q_jC__);
+ (*this)(1,0) = - cos__q_jC__;
+ (*this)(1,1) = - sin__q_jC__;
+ (*this)(1,5) = (- 1.095 * sin__q_jC__);
+ (*this)(3,3) = - sin__q_jC__;
+ (*this)(3,4) = cos__q_jC__;
+ (*this)(4,3) = - cos__q_jC__;
+ (*this)(4,4) = - sin__q_jC__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link2_X_fr_link3::Type_fr_link2_X_fr_link3()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = - 1.095;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1.0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link2_X_fr_link3& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link2_X_fr_link3::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR cos__q_jC__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+
+ (*this)(0,0) = - sin__q_jC__;
+ (*this)(0,1) = - cos__q_jC__;
+ (*this)(1,0) = cos__q_jC__;
+ (*this)(1,1) = - sin__q_jC__;
+ (*this)(2,3) = ( 1.095 * cos__q_jC__);
+ (*this)(2,4) = (- 1.095 * sin__q_jC__);
+ (*this)(3,3) = - sin__q_jC__;
+ (*this)(3,4) = - cos__q_jC__;
+ (*this)(4,3) = cos__q_jC__;
+ (*this)(4,4) = - sin__q_jC__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link4_X_fr_link3::Type_fr_link4_X_fr_link3()
+{
+ (*this)(0,0) = 0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0.175;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 1.0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link4_X_fr_link3& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link4_X_fr_link3::update(const JState& q) {
+ SCALAR sin__q_jD__;
+ SCALAR cos__q_jD__;
+
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+
+ (*this)(0,1) = sin__q_jD__;
+ (*this)(0,2) = - cos__q_jD__;
+ (*this)(0,3) = ( 0.175 * cos__q_jD__);
+ (*this)(0,4) = ( 0.332 * cos__q_jD__);
+ (*this)(0,5) = ( 0.332 * sin__q_jD__);
+ (*this)(1,1) = cos__q_jD__;
+ (*this)(1,2) = sin__q_jD__;
+ (*this)(1,3) = (- 0.175 * sin__q_jD__);
+ (*this)(1,4) = (- 0.332 * sin__q_jD__);
+ (*this)(1,5) = ( 0.332 * cos__q_jD__);
+ (*this)(3,4) = sin__q_jD__;
+ (*this)(3,5) = - cos__q_jD__;
+ (*this)(4,4) = cos__q_jD__;
+ (*this)(4,5) = sin__q_jD__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link3_X_fr_link4::Type_fr_link3_X_fr_link4()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 1.0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,5) = 0.175;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 1.0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link3_X_fr_link4& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link3_X_fr_link4::update(const JState& q) {
+ SCALAR sin__q_jD__;
+ SCALAR cos__q_jD__;
+
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+
+ (*this)(0,3) = ( 0.175 * cos__q_jD__);
+ (*this)(0,4) = (- 0.175 * sin__q_jD__);
+ (*this)(1,0) = sin__q_jD__;
+ (*this)(1,1) = cos__q_jD__;
+ (*this)(1,3) = ( 0.332 * cos__q_jD__);
+ (*this)(1,4) = (- 0.332 * sin__q_jD__);
+ (*this)(2,0) = - cos__q_jD__;
+ (*this)(2,1) = sin__q_jD__;
+ (*this)(2,3) = ( 0.332 * sin__q_jD__);
+ (*this)(2,4) = ( 0.332 * cos__q_jD__);
+ (*this)(4,3) = sin__q_jD__;
+ (*this)(4,4) = cos__q_jD__;
+ (*this)(5,3) = - cos__q_jD__;
+ (*this)(5,4) = sin__q_jD__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link5_X_fr_link4::Type_fr_link5_X_fr_link4()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = - 1;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = - 0.939;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = - 1.0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link5_X_fr_link4& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link5_X_fr_link4::update(const JState& q) {
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jE__;
+
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+
+ (*this)(0,1) = sin__q_jE__;
+ (*this)(0,2) = cos__q_jE__;
+ (*this)(0,3) = (- 0.939 * sin__q_jE__);
+ (*this)(1,1) = cos__q_jE__;
+ (*this)(1,2) = - sin__q_jE__;
+ (*this)(1,3) = (- 0.939 * cos__q_jE__);
+ (*this)(3,4) = sin__q_jE__;
+ (*this)(3,5) = cos__q_jE__;
+ (*this)(4,4) = cos__q_jE__;
+ (*this)(4,5) = - sin__q_jE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link4_X_fr_link5::Type_fr_link4_X_fr_link5()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = - 1.0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = - 0.939;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = - 1;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link4_X_fr_link5& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link4_X_fr_link5::update(const JState& q) {
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jE__;
+
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+
+ (*this)(0,3) = (- 0.939 * sin__q_jE__);
+ (*this)(0,4) = (- 0.939 * cos__q_jE__);
+ (*this)(1,0) = sin__q_jE__;
+ (*this)(1,1) = cos__q_jE__;
+ (*this)(2,0) = cos__q_jE__;
+ (*this)(2,1) = - sin__q_jE__;
+ (*this)(4,3) = sin__q_jE__;
+ (*this)(4,4) = cos__q_jE__;
+ (*this)(5,3) = cos__q_jE__;
+ (*this)(5,4) = - sin__q_jE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link6_X_fr_link5::Type_fr_link6_X_fr_link5()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,3) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,3) = 0;
+ (*this)(2,0) = 1;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 1;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link6_X_fr_link5& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link6_X_fr_link5::update(const JState& q) {
+ SCALAR sin__q_jF__;
+ SCALAR cos__q_jF__;
+
+ sin__q_jF__ = TRAIT::sin( q(JF));
+ cos__q_jF__ = TRAIT::cos( q(JF));
+
+ (*this)(0,1) = cos__q_jF__;
+ (*this)(0,2) = sin__q_jF__;
+ (*this)(0,4) = (- 0.243 * sin__q_jF__);
+ (*this)(0,5) = ( 0.243 * cos__q_jF__);
+ (*this)(1,1) = - sin__q_jF__;
+ (*this)(1,2) = cos__q_jF__;
+ (*this)(1,4) = (- 0.243 * cos__q_jF__);
+ (*this)(1,5) = (- 0.243 * sin__q_jF__);
+ (*this)(3,4) = cos__q_jF__;
+ (*this)(3,5) = sin__q_jF__;
+ (*this)(4,4) = - sin__q_jF__;
+ (*this)(4,5) = cos__q_jF__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link5_X_fr_link6::Type_fr_link5_X_fr_link6()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 1;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 1;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link5_X_fr_link6& iit::testirb4600::tpl::ForceTransforms<TRAIT>::Type_fr_link5_X_fr_link6::update(const JState& q) {
+ SCALAR sin__q_jF__;
+ SCALAR cos__q_jF__;
+
+ sin__q_jF__ = TRAIT::sin( q(JF));
+ cos__q_jF__ = TRAIT::cos( q(JF));
+
+ (*this)(1,0) = cos__q_jF__;
+ (*this)(1,1) = - sin__q_jF__;
+ (*this)(1,3) = (- 0.243 * sin__q_jF__);
+ (*this)(1,4) = (- 0.243 * cos__q_jF__);
+ (*this)(2,0) = sin__q_jF__;
+ (*this)(2,1) = cos__q_jF__;
+ (*this)(2,3) = ( 0.243 * cos__q_jF__);
+ (*this)(2,4) = (- 0.243 * sin__q_jF__);
+ (*this)(4,3) = cos__q_jF__;
+ (*this)(4,4) = - sin__q_jF__;
+ (*this)(5,3) = sin__q_jF__;
+ (*this)(5,4) = cos__q_jF__;
+ return *this;
+}
+
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_link1::Type_fr_link0_X_fr_link1()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_link1& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_link1::update(const JState& q) {
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+
+ (*this)(0,0) = cos__q_jA__;
+ (*this)(0,1) = - sin__q_jA__;
+ (*this)(1,0) = sin__q_jA__;
+ (*this)(1,1) = cos__q_jA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_link2::Type_fr_link0_X_fr_link2()
+{
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0.495;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_link2& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_link2::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+
+ (*this)(0,0) = ( cos__q_jA__ * sin__q_jB__);
+ (*this)(0,1) = ( cos__q_jA__ * cos__q_jB__);
+ (*this)(0,2) = - sin__q_jA__;
+ (*this)(0,3) = ( 0.175 * cos__q_jA__);
+ (*this)(1,0) = ( sin__q_jA__ * sin__q_jB__);
+ (*this)(1,1) = ( sin__q_jA__ * cos__q_jB__);
+ (*this)(1,2) = cos__q_jA__;
+ (*this)(1,3) = ( 0.175 * sin__q_jA__);
+ (*this)(2,0) = cos__q_jB__;
+ (*this)(2,1) = - sin__q_jB__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_link3::Type_fr_link0_X_fr_link3()
+{
+ (*this)(2,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_link3& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_link3::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+
+ (*this)(0,0) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(0,1) = (((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(0,2) = - sin__q_jA__;
+ (*this)(0,3) = ((( 1.095 * cos__q_jA__) * sin__q_jB__) + ( 0.175 * cos__q_jA__));
+ (*this)(1,0) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(1,1) = (((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(1,2) = cos__q_jA__;
+ (*this)(1,3) = ((( 1.095 * sin__q_jA__) * sin__q_jB__) + ( 0.175 * sin__q_jA__));
+ (*this)(2,0) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(2,1) = (( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__));
+ (*this)(2,3) = (( 1.095 * cos__q_jB__) + 0.495);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_link4::Type_fr_link0_X_fr_link4()
+{
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_link4& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_link4::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+
+ (*this)(0,0) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( sin__q_jA__ * cos__q_jD__));
+ (*this)(0,1) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__));
+ (*this)(0,2) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(0,3) = ((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.332 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.332 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * sin__q_jB__)) + ( 0.175 * cos__q_jA__));
+ (*this)(1,0) = (((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( cos__q_jA__ * cos__q_jD__));
+ (*this)(1,1) = (( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__));
+ (*this)(1,2) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(1,3) = ((((((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.332 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.332 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * sin__q_jB__)) + ( 0.175 * sin__q_jA__));
+ (*this)(2,0) = ((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * sin__q_jD__);
+ (*this)(2,1) = ((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__);
+ (*this)(2,2) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(2,3) = ((((((- 0.175 * sin__q_jB__) - ( 0.332 * cos__q_jB__)) * sin__q_jC__) + ((( 0.175 * cos__q_jB__) - ( 0.332 * sin__q_jB__)) * cos__q_jC__)) + ( 1.095 * cos__q_jB__)) + 0.495);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_link5::Type_fr_link0_X_fr_link5()
+{
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_link5& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_link5::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+
+ (*this)(0,0) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(0,1) = ((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__));
+ (*this)(0,2) = ((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(0,3) = ((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 1.271 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * sin__q_jB__)) + ( 0.175 * cos__q_jA__));
+ (*this)(1,0) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(1,1) = ((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(1,2) = ((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(1,3) = ((((((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 1.271 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * sin__q_jB__)) + ( 0.175 * sin__q_jA__));
+ (*this)(2,0) = ((((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(2,1) = (((( cos__q_jB__ * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__));
+ (*this)(2,2) = ((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ (*this)(2,3) = ((((((- 0.175 * sin__q_jB__) - ( 1.271 * cos__q_jB__)) * sin__q_jC__) + ((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * cos__q_jC__)) + ( 1.095 * cos__q_jB__)) + 0.495);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_link6::Type_fr_link0_X_fr_link6()
+{
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_link6& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_link6::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jF__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+ SCALAR cos__q_jF__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jF__ = TRAIT::sin( q(JF));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+ cos__q_jF__ = TRAIT::cos( q(JF));
+
+ (*this)(0,0) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,1) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(0,2) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + ((((( 1.0 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(0,3) = ((((((((((((- 0.243 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.243 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.243 * sin__q_jA__) * sin__q_jD__)) * sin__q_jE__) + ((((( 0.243 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.243 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__)) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 1.271 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__)) + (((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * sin__q_jB__)) + ( 0.175 * cos__q_jA__));
+ (*this)(1,0) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(1,1) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,2) = ((((( 1.0 * cos__q_jA__) * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(1,3) = ((((((((( 0.243 * cos__q_jA__) * sin__q_jD__) + (((((- 0.243 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.243 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + ((((( 0.243 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.243 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__)) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 1.271 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__)) + (((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * sin__q_jB__)) + ( 0.175 * sin__q_jA__));
+ (*this)(2,0) = ((((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + ((((( cos__q_jB__ * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(2,1) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(2,2) = (((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(2,3) = (((((((((( 0.243 * sin__q_jB__) * sin__q_jC__) - (( 0.243 * cos__q_jB__) * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + ((((- 0.243 * cos__q_jB__) * sin__q_jC__) - (( 0.243 * sin__q_jB__) * cos__q_jC__)) * cos__q_jE__)) + (((- 0.175 * sin__q_jB__) - ( 1.271 * cos__q_jB__)) * sin__q_jC__)) + ((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * cos__q_jC__)) + ( 1.095 * cos__q_jB__)) + 0.495);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_ee::Type_fr_link0_X_fr_ee()
+{
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_ee& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_ee::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jF__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+ SCALAR cos__q_jF__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jF__ = TRAIT::sin( q(JF));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+ cos__q_jF__ = TRAIT::cos( q(JF));
+
+ (*this)(0,0) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,1) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(0,2) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + ((((( 1.0 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(0,3) = ((((((((((((- 0.443 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.443 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.443 * sin__q_jA__) * sin__q_jD__)) * sin__q_jE__) + ((((( 0.443 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.443 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__)) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 1.271 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__)) + (((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * sin__q_jB__)) + ( 0.175 * cos__q_jA__));
+ (*this)(1,0) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(1,1) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,2) = ((((( 1.0 * cos__q_jA__) * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(1,3) = ((((((((( 0.443 * cos__q_jA__) * sin__q_jD__) + (((((- 0.443 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.443 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + ((((( 0.443 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.443 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__)) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 1.271 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__)) + (((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * sin__q_jB__)) + ( 0.175 * sin__q_jA__));
+ (*this)(2,0) = ((((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + ((((( cos__q_jB__ * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(2,1) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(2,2) = (((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(2,3) = (((((((((( 0.443 * sin__q_jB__) * sin__q_jC__) - (( 0.443 * cos__q_jB__) * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + ((((- 0.443 * cos__q_jB__) * sin__q_jC__) - (( 0.443 * sin__q_jB__) * cos__q_jC__)) * cos__q_jE__)) + (((- 0.175 * sin__q_jB__) - ( 1.271 * cos__q_jB__)) * sin__q_jC__)) + ((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * cos__q_jC__)) + ( 1.095 * cos__q_jB__)) + 0.495);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_com0::Type_fr_link0_X_com0()
+{
+ (*this)(0,0) = 1;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 1;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0.075;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_com0& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_com0::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_com1::Type_fr_link0_X_com1()
+{
+ (*this)(0,2) = 0;
+ (*this)(1,2) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1.0;
+ (*this)(2,3) = 0.25;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_com1& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_com1::update(const JState& q) {
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+
+ (*this)(0,0) = cos__q_jA__;
+ (*this)(0,1) = - sin__q_jA__;
+ (*this)(0,3) = ( 0.09 * cos__q_jA__);
+ (*this)(1,0) = sin__q_jA__;
+ (*this)(1,1) = cos__q_jA__;
+ (*this)(1,3) = ( 0.09 * sin__q_jA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_com2::Type_fr_link0_X_com2()
+{
+ (*this)(2,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_com2& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_com2::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+
+ (*this)(0,0) = ( cos__q_jA__ * sin__q_jB__);
+ (*this)(0,1) = ( cos__q_jA__ * cos__q_jB__);
+ (*this)(0,2) = - sin__q_jA__;
+ (*this)(0,3) = ((((( 0.47 * cos__q_jA__) * sin__q_jB__) + (( 0.04 * cos__q_jA__) * cos__q_jB__)) + ( 0.17 * sin__q_jA__)) + ( 0.175 * cos__q_jA__));
+ (*this)(1,0) = ( sin__q_jA__ * sin__q_jB__);
+ (*this)(1,1) = ( sin__q_jA__ * cos__q_jB__);
+ (*this)(1,2) = cos__q_jA__;
+ (*this)(1,3) = ((((( 0.47 * sin__q_jA__) * sin__q_jB__) + (( 0.04 * sin__q_jA__) * cos__q_jB__)) + ( 0.175 * sin__q_jA__)) - ( 0.17 * cos__q_jA__));
+ (*this)(2,0) = cos__q_jB__;
+ (*this)(2,1) = - sin__q_jB__;
+ (*this)(2,3) = (((- 0.04 * sin__q_jB__) + ( 0.47 * cos__q_jB__)) + 0.495);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_com3::Type_fr_link0_X_com3()
+{
+ (*this)(2,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_com3& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_com3::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+
+ (*this)(0,0) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(0,1) = (((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(0,2) = - sin__q_jA__;
+ (*this)(0,3) = (((((((( 0.133 * cos__q_jA__) * cos__q_jB__) - (( 0.035 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + (((( 0.133 * cos__q_jA__) * sin__q_jB__) + (( 0.035 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * sin__q_jB__)) - ( 0.044 * sin__q_jA__)) + ( 0.175 * cos__q_jA__));
+ (*this)(1,0) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(1,1) = (((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(1,2) = cos__q_jA__;
+ (*this)(1,3) = (((((((( 0.133 * sin__q_jA__) * cos__q_jB__) - (( 0.035 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__) + (((( 0.133 * sin__q_jA__) * sin__q_jB__) + (( 0.035 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * sin__q_jB__)) + ( 0.175 * sin__q_jA__)) + ( 0.044 * cos__q_jA__));
+ (*this)(2,0) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(2,1) = (( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__));
+ (*this)(2,3) = ((((((- 0.133 * sin__q_jB__) - ( 0.035 * cos__q_jB__)) * sin__q_jC__) + ((( 0.133 * cos__q_jB__) - ( 0.035 * sin__q_jB__)) * cos__q_jC__)) + ( 1.095 * cos__q_jB__)) + 0.495);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_com4::Type_fr_link0_X_com4()
+{
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_com4& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_com4::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+
+ (*this)(0,0) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( sin__q_jA__ * cos__q_jD__));
+ (*this)(0,1) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__));
+ (*this)(0,2) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(0,3) = ((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.832 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.832 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * sin__q_jB__)) + ( 0.175 * cos__q_jA__));
+ (*this)(1,0) = (((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( cos__q_jA__ * cos__q_jD__));
+ (*this)(1,1) = (( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__));
+ (*this)(1,2) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(1,3) = ((((((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.832 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.832 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * sin__q_jB__)) + ( 0.175 * sin__q_jA__));
+ (*this)(2,0) = ((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * sin__q_jD__);
+ (*this)(2,1) = ((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__);
+ (*this)(2,2) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(2,3) = ((((((- 0.175 * sin__q_jB__) - ( 0.832 * cos__q_jB__)) * sin__q_jC__) + ((( 0.175 * cos__q_jB__) - ( 0.832 * sin__q_jB__)) * cos__q_jC__)) + ( 1.095 * cos__q_jB__)) + 0.495);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_com5::Type_fr_link0_X_com5()
+{
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_com5& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_com5::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+
+ (*this)(0,0) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(0,1) = ((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__));
+ (*this)(0,2) = ((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(0,3) = ((((((((((((- 0.07 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.07 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.07 * sin__q_jA__) * sin__q_jD__)) * sin__q_jE__) + ((((( 0.07 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.07 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__)) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 1.271 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__)) + (((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * sin__q_jB__)) + ( 0.175 * cos__q_jA__));
+ (*this)(1,0) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(1,1) = ((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(1,2) = ((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(1,3) = ((((((((( 0.07 * cos__q_jA__) * sin__q_jD__) + (((((- 0.07 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.07 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + ((((( 0.07 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.07 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__)) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 1.271 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__)) + (((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * sin__q_jB__)) + ( 0.175 * sin__q_jA__));
+ (*this)(2,0) = ((((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(2,1) = (((( cos__q_jB__ * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__));
+ (*this)(2,2) = ((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ (*this)(2,3) = (((((((((( 0.07 * sin__q_jB__) * sin__q_jC__) - (( 0.07 * cos__q_jB__) * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + ((((- 0.07 * cos__q_jB__) * sin__q_jC__) - (( 0.07 * sin__q_jB__) * cos__q_jC__)) * cos__q_jE__)) + (((- 0.175 * sin__q_jB__) - ( 1.271 * cos__q_jB__)) * sin__q_jC__)) + ((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * cos__q_jC__)) + ( 1.095 * cos__q_jB__)) + 0.495);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_com6::Type_fr_link0_X_com6()
+{
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_com6& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_com6::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jF__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+ SCALAR cos__q_jF__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jF__ = TRAIT::sin( q(JF));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+ cos__q_jF__ = TRAIT::cos( q(JF));
+
+ (*this)(0,0) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,1) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(0,2) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + ((((( 1.0 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(0,3) = ((((((((((((- 0.3429 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.3429 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.3429 * sin__q_jA__) * sin__q_jD__)) * sin__q_jE__) + ((((( 0.3429 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.3429 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__)) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 1.271 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__)) + (((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * sin__q_jB__)) + ( 0.175 * cos__q_jA__));
+ (*this)(1,0) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(1,1) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,2) = ((((( 1.0 * cos__q_jA__) * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(1,3) = ((((((((( 0.3429 * cos__q_jA__) * sin__q_jD__) + (((((- 0.3429 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.3429 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + ((((( 0.3429 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.3429 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__)) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 1.271 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__)) + (((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * sin__q_jB__)) + ( 0.175 * sin__q_jA__));
+ (*this)(2,0) = ((((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + ((((( cos__q_jB__ * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(2,1) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + (((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(2,2) = (((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(2,3) = (((((((((( 0.3429 * sin__q_jB__) * sin__q_jC__) - (( 0.3429 * cos__q_jB__) * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + ((((- 0.3429 * cos__q_jB__) * sin__q_jC__) - (( 0.3429 * sin__q_jB__) * cos__q_jC__)) * cos__q_jE__)) + (((- 0.175 * sin__q_jB__) - ( 1.271 * cos__q_jB__)) * sin__q_jC__)) + ((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * cos__q_jC__)) + ( 1.095 * cos__q_jB__)) + 0.495);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link1_X_fr_link0::Type_fr_link1_X_fr_link0()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link1_X_fr_link0& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link1_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+
+ (*this)(0,0) = cos__q_jA__;
+ (*this)(0,1) = sin__q_jA__;
+ (*this)(1,0) = - sin__q_jA__;
+ (*this)(1,1) = cos__q_jA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link2_X_fr_link0::Type_fr_link2_X_fr_link0()
+{
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link2_X_fr_link0& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link2_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+
+ (*this)(0,0) = ( cos__q_jA__ * sin__q_jB__);
+ (*this)(0,1) = ( sin__q_jA__ * sin__q_jB__);
+ (*this)(0,2) = cos__q_jB__;
+ (*this)(0,3) = ((- 0.175 * sin__q_jB__) - ( 0.495 * cos__q_jB__));
+ (*this)(1,0) = ( cos__q_jA__ * cos__q_jB__);
+ (*this)(1,1) = ( sin__q_jA__ * cos__q_jB__);
+ (*this)(1,2) = - sin__q_jB__;
+ (*this)(1,3) = (( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__));
+ (*this)(2,0) = - sin__q_jA__;
+ (*this)(2,1) = cos__q_jA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link3_X_fr_link0::Type_fr_link3_X_fr_link0()
+{
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link3_X_fr_link0& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link3_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+
+ (*this)(0,0) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(0,1) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(0,2) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(0,3) = ((((( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__)) + 1.095) * sin__q_jC__) + ((( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * cos__q_jC__));
+ (*this)(1,0) = (((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(1,1) = (((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(1,2) = (( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__));
+ (*this)(1,3) = (((( 0.175 * cos__q_jB__) - ( 0.495 * sin__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__)) + 1.095) * cos__q_jC__));
+ (*this)(2,0) = - sin__q_jA__;
+ (*this)(2,1) = cos__q_jA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link4_X_fr_link0::Type_fr_link4_X_fr_link0()
+{
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link4_X_fr_link0& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link4_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+
+ (*this)(0,0) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( sin__q_jA__ * cos__q_jD__));
+ (*this)(0,1) = (((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( cos__q_jA__ * cos__q_jD__));
+ (*this)(0,2) = (((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * sin__q_jD__);
+ (*this)(0,3) = (((((( 0.175 * cos__q_jB__) - ( 0.495 * sin__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__)) + 1.095) * cos__q_jC__)) + 0.175) * sin__q_jD__);
+ (*this)(1,0) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__));
+ (*this)(1,1) = (( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__));
+ (*this)(1,2) = (((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__);
+ (*this)(1,3) = (((((( 0.175 * cos__q_jB__) - ( 0.495 * sin__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__)) + 1.095) * cos__q_jC__)) + 0.175) * cos__q_jD__);
+ (*this)(2,0) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(2,1) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(2,2) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(2,3) = (((((( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__)) + 1.095) * sin__q_jC__) + ((( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * cos__q_jC__)) - 0.332);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link5_X_fr_link0::Type_fr_link5_X_fr_link0()
+{
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link5_X_fr_link0& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link5_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+
+ (*this)(0,0) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(0,1) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(0,2) = (((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(0,3) = (((((((( 0.175 * cos__q_jB__) - ( 0.495 * sin__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__)) + 1.095) * cos__q_jC__)) + 0.175) * cos__q_jD__) * sin__q_jE__) + ((((((( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__)) + 1.095) * sin__q_jC__) + ((( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * cos__q_jC__)) - 1.271) * cos__q_jE__));
+ (*this)(1,0) = ((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__));
+ (*this)(1,1) = ((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(1,2) = ((((( 1.0 * cos__q_jB__) * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__));
+ (*this)(1,3) = ((((((((- 0.175 * sin__q_jB__) - ( 0.495 * cos__q_jB__)) - 1.095) * sin__q_jC__) + ((( 0.175 * cos__q_jB__) - ( 0.495 * sin__q_jB__)) * cos__q_jC__)) + 1.271) * sin__q_jE__) + ((((((( 0.175 * cos__q_jB__) - ( 0.495 * sin__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__)) + 1.095) * cos__q_jC__)) + 0.175) * cos__q_jD__) * cos__q_jE__));
+ (*this)(2,0) = ((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(2,1) = ((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(2,2) = (((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ (*this)(2,3) = (((((( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + ((((- 0.175 * sin__q_jB__) - ( 0.495 * cos__q_jB__)) - 1.095) * cos__q_jC__)) - 0.175) * sin__q_jD__);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link6_X_fr_link0::Type_fr_link6_X_fr_link0()
+{
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link6_X_fr_link0& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link6_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jF__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+ SCALAR cos__q_jF__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jF__ = TRAIT::sin( q(JF));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+ cos__q_jF__ = TRAIT::cos( q(JF));
+
+ (*this)(0,0) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,1) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,2) = (((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + (((((( 1.0 * cos__q_jB__) * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,3) = (((((((( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + ((((- 0.175 * sin__q_jB__) - ( 0.495 * cos__q_jB__)) - 1.095) * cos__q_jC__)) - 0.175) * sin__q_jD__) * sin__q_jF__) + (((((((((- 0.175 * sin__q_jB__) - ( 0.495 * cos__q_jB__)) - 1.095) * sin__q_jC__) + ((( 0.175 * cos__q_jB__) - ( 0.495 * sin__q_jB__)) * cos__q_jC__)) + 1.271) * sin__q_jE__) + ((((((( 0.175 * cos__q_jB__) - ( 0.495 * sin__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__)) + 1.095) * cos__q_jC__)) + 0.175) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(1,0) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,1) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,2) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(1,3) = (((((((((( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__)) + 1.095) * sin__q_jC__) + ((( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * cos__q_jC__)) - 1.271) * sin__q_jE__) + ((((((( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + ((((- 0.175 * sin__q_jB__) - ( 0.495 * cos__q_jB__)) - 1.095) * cos__q_jC__)) - 0.175) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + ((((((( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + ((((- 0.175 * sin__q_jB__) - ( 0.495 * cos__q_jB__)) - 1.095) * cos__q_jC__)) - 0.175) * sin__q_jD__) * cos__q_jF__));
+ (*this)(2,0) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(2,1) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(2,2) = (((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(2,3) = ((((((((( 0.175 * cos__q_jB__) - ( 0.495 * sin__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__)) + 1.095) * cos__q_jC__)) + 0.175) * cos__q_jD__) * sin__q_jE__) + ((((((( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__)) + 1.095) * sin__q_jC__) + ((( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * cos__q_jC__)) - 1.271) * cos__q_jE__)) - 0.243);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_ee_X_fr_link0::Type_fr_ee_X_fr_link0()
+{
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_ee_X_fr_link0& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_ee_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jF__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+ SCALAR cos__q_jF__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jF__ = TRAIT::sin( q(JF));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+ cos__q_jF__ = TRAIT::cos( q(JF));
+
+ (*this)(0,0) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,1) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,2) = (((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + (((((( 1.0 * cos__q_jB__) * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,3) = (((((((( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + ((((- 0.175 * sin__q_jB__) - ( 0.495 * cos__q_jB__)) - 1.095) * cos__q_jC__)) - 0.175) * sin__q_jD__) * sin__q_jF__) + (((((((((- 0.175 * sin__q_jB__) - ( 0.495 * cos__q_jB__)) - 1.095) * sin__q_jC__) + ((( 0.175 * cos__q_jB__) - ( 0.495 * sin__q_jB__)) * cos__q_jC__)) + 1.271) * sin__q_jE__) + ((((((( 0.175 * cos__q_jB__) - ( 0.495 * sin__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__)) + 1.095) * cos__q_jC__)) + 0.175) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(1,0) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,1) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,2) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(1,3) = (((((((((( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__)) + 1.095) * sin__q_jC__) + ((( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * cos__q_jC__)) - 1.271) * sin__q_jE__) + ((((((( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + ((((- 0.175 * sin__q_jB__) - ( 0.495 * cos__q_jB__)) - 1.095) * cos__q_jC__)) - 0.175) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + ((((((( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + ((((- 0.175 * sin__q_jB__) - ( 0.495 * cos__q_jB__)) - 1.095) * cos__q_jC__)) - 0.175) * sin__q_jD__) * cos__q_jF__));
+ (*this)(2,0) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(2,1) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(2,2) = (((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(2,3) = ((((((((( 0.175 * cos__q_jB__) - ( 0.495 * sin__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__)) + 1.095) * cos__q_jC__)) + 0.175) * cos__q_jD__) * sin__q_jE__) + ((((((( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__)) + 1.095) * sin__q_jC__) + ((( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * cos__q_jC__)) - 1.271) * cos__q_jE__)) - 0.443);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_com0_X_fr_link0::Type_com0_X_fr_link0()
+{
+ (*this)(0,0) = 1;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 1;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = - 0.075;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_com0_X_fr_link0& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_com0_X_fr_link0::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_com1_X_fr_link0::Type_com1_X_fr_link0()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = - 0.09;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1.0;
+ (*this)(2,3) = - 0.25;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_com1_X_fr_link0& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_com1_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+
+ (*this)(0,0) = cos__q_jA__;
+ (*this)(0,1) = sin__q_jA__;
+ (*this)(1,0) = - sin__q_jA__;
+ (*this)(1,1) = cos__q_jA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_com2_X_fr_link0::Type_com2_X_fr_link0()
+{
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0.17;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_com2_X_fr_link0& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_com2_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+
+ (*this)(0,0) = ( cos__q_jA__ * sin__q_jB__);
+ (*this)(0,1) = ( sin__q_jA__ * sin__q_jB__);
+ (*this)(0,2) = cos__q_jB__;
+ (*this)(0,3) = (((- 0.175 * sin__q_jB__) - ( 0.495 * cos__q_jB__)) - 0.47);
+ (*this)(1,0) = ( cos__q_jA__ * cos__q_jB__);
+ (*this)(1,1) = ( sin__q_jA__ * cos__q_jB__);
+ (*this)(1,2) = - sin__q_jB__;
+ (*this)(1,3) = ((( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) - 0.04);
+ (*this)(2,0) = - sin__q_jA__;
+ (*this)(2,1) = cos__q_jA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_com3_X_fr_link0::Type_com3_X_fr_link0()
+{
+ (*this)(2,2) = 0;
+ (*this)(2,3) = - 0.044;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_com3_X_fr_link0& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_com3_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+
+ (*this)(0,0) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(0,1) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(0,2) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(0,3) = (((((( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__)) + 1.095) * sin__q_jC__) + ((( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * cos__q_jC__)) - 0.035);
+ (*this)(1,0) = (((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(1,1) = (((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(1,2) = (( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__));
+ (*this)(1,3) = ((((( 0.175 * cos__q_jB__) - ( 0.495 * sin__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__)) + 1.095) * cos__q_jC__)) + 0.133);
+ (*this)(2,0) = - sin__q_jA__;
+ (*this)(2,1) = cos__q_jA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_com4_X_fr_link0::Type_com4_X_fr_link0()
+{
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_com4_X_fr_link0& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_com4_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+
+ (*this)(0,0) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( sin__q_jA__ * cos__q_jD__));
+ (*this)(0,1) = (((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( cos__q_jA__ * cos__q_jD__));
+ (*this)(0,2) = (((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * sin__q_jD__);
+ (*this)(0,3) = (((((( 0.175 * cos__q_jB__) - ( 0.495 * sin__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__)) + 1.095) * cos__q_jC__)) + 0.175) * sin__q_jD__);
+ (*this)(1,0) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__));
+ (*this)(1,1) = (( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__));
+ (*this)(1,2) = (((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__);
+ (*this)(1,3) = (((((( 0.175 * cos__q_jB__) - ( 0.495 * sin__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__)) + 1.095) * cos__q_jC__)) + 0.175) * cos__q_jD__);
+ (*this)(2,0) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(2,1) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(2,2) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(2,3) = (((((( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__)) + 1.095) * sin__q_jC__) + ((( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * cos__q_jC__)) - 0.832);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_com5_X_fr_link0::Type_com5_X_fr_link0()
+{
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_com5_X_fr_link0& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_com5_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+
+ (*this)(0,0) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(0,1) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(0,2) = (((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(0,3) = ((((((((( 0.175 * cos__q_jB__) - ( 0.495 * sin__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__)) + 1.095) * cos__q_jC__)) + 0.175) * cos__q_jD__) * sin__q_jE__) + ((((((( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__)) + 1.095) * sin__q_jC__) + ((( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * cos__q_jC__)) - 1.271) * cos__q_jE__)) - 0.07);
+ (*this)(1,0) = ((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__));
+ (*this)(1,1) = ((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(1,2) = ((((( 1.0 * cos__q_jB__) * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__));
+ (*this)(1,3) = ((((((((- 0.175 * sin__q_jB__) - ( 0.495 * cos__q_jB__)) - 1.095) * sin__q_jC__) + ((( 0.175 * cos__q_jB__) - ( 0.495 * sin__q_jB__)) * cos__q_jC__)) + 1.271) * sin__q_jE__) + ((((((( 0.175 * cos__q_jB__) - ( 0.495 * sin__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__)) + 1.095) * cos__q_jC__)) + 0.175) * cos__q_jD__) * cos__q_jE__));
+ (*this)(2,0) = ((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(2,1) = ((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(2,2) = (((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ (*this)(2,3) = (((((( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + ((((- 0.175 * sin__q_jB__) - ( 0.495 * cos__q_jB__)) - 1.095) * cos__q_jC__)) - 0.175) * sin__q_jD__);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_com6_X_fr_link0::Type_com6_X_fr_link0()
+{
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_com6_X_fr_link0& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_com6_X_fr_link0::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jD__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jF__;
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+ SCALAR cos__q_jF__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jF__ = TRAIT::sin( q(JF));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+ cos__q_jF__ = TRAIT::cos( q(JF));
+
+ (*this)(0,0) = ((((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,1) = ((((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * sin__q_jF__) + (((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,2) = (((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * sin__q_jF__) + (((((( 1.0 * cos__q_jB__) * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(0,3) = (((((((( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + ((((- 0.175 * sin__q_jB__) - ( 0.495 * cos__q_jB__)) - 1.095) * cos__q_jC__)) - 0.175) * sin__q_jD__) * sin__q_jF__) + (((((((((- 0.175 * sin__q_jB__) - ( 0.495 * cos__q_jB__)) - 1.095) * sin__q_jC__) + ((( 0.175 * cos__q_jB__) - ( 0.495 * sin__q_jB__)) * cos__q_jC__)) + 1.271) * sin__q_jE__) + ((((((( 0.175 * cos__q_jB__) - ( 0.495 * sin__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__)) + 1.095) * cos__q_jC__)) + 0.175) * cos__q_jD__) * cos__q_jE__)) * cos__q_jF__));
+ (*this)(1,0) = ((((((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + ((( sin__q_jA__ * sin__q_jD__) + (((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,1) = ((((((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * sin__q_jE__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( cos__q_jA__ * sin__q_jD__)) * cos__q_jE__)) * sin__q_jF__) + (((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__)) * cos__q_jF__));
+ (*this)(1,2) = ((((((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + ((((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__) * cos__q_jF__));
+ (*this)(1,3) = (((((((((( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__)) + 1.095) * sin__q_jC__) + ((( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * cos__q_jC__)) - 1.271) * sin__q_jE__) + ((((((( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + ((((- 0.175 * sin__q_jB__) - ( 0.495 * cos__q_jB__)) - 1.095) * cos__q_jC__)) - 0.175) * cos__q_jD__) * cos__q_jE__)) * sin__q_jF__) + ((((((( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * sin__q_jC__) + ((((- 0.175 * sin__q_jB__) - ( 0.495 * cos__q_jB__)) - 1.095) * cos__q_jC__)) - 0.175) * sin__q_jD__) * cos__q_jF__));
+ (*this)(2,0) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + (((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(2,1) = (((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + (((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(2,2) = (((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(2,3) = ((((((((( 0.175 * cos__q_jB__) - ( 0.495 * sin__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__)) + 1.095) * cos__q_jC__)) + 0.175) * cos__q_jD__) * sin__q_jE__) + ((((((( 0.175 * sin__q_jB__) + ( 0.495 * cos__q_jB__)) + 1.095) * sin__q_jC__) + ((( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__)) * cos__q_jC__)) - 1.271) * cos__q_jE__)) - 0.3429);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_jA::Type_fr_link0_X_fr_jA()
+{
+ (*this)(0,0) = 1;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 1.0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1.0;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_jA& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_jA::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_jB::Type_fr_link0_X_fr_jB()
+{
+ (*this)(0,0) = 0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0.495;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_jB& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_jB::update(const JState& q) {
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+
+ (*this)(0,1) = cos__q_jA__;
+ (*this)(0,2) = - sin__q_jA__;
+ (*this)(0,3) = ( 0.175 * cos__q_jA__);
+ (*this)(1,1) = sin__q_jA__;
+ (*this)(1,2) = cos__q_jA__;
+ (*this)(1,3) = ( 0.175 * sin__q_jA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_jC::Type_fr_link0_X_fr_jC()
+{
+ (*this)(2,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_jC& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_jC::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jA__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+
+ (*this)(0,0) = ( cos__q_jA__ * cos__q_jB__);
+ (*this)(0,1) = (- cos__q_jA__ * sin__q_jB__);
+ (*this)(0,2) = - sin__q_jA__;
+ (*this)(0,3) = ((( 1.095 * cos__q_jA__) * sin__q_jB__) + ( 0.175 * cos__q_jA__));
+ (*this)(1,0) = ( sin__q_jA__ * cos__q_jB__);
+ (*this)(1,1) = (- sin__q_jA__ * sin__q_jB__);
+ (*this)(1,2) = cos__q_jA__;
+ (*this)(1,3) = ((( 1.095 * sin__q_jA__) * sin__q_jB__) + ( 0.175 * sin__q_jA__));
+ (*this)(2,0) = - sin__q_jB__;
+ (*this)(2,1) = - cos__q_jB__;
+ (*this)(2,3) = (( 1.095 * cos__q_jB__) + 0.495);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_jD::Type_fr_link0_X_fr_jD()
+{
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_jD& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_jD::update(const JState& q) {
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jB__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+
+ (*this)(0,0) = sin__q_jA__;
+ (*this)(0,1) = (((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(0,2) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(0,3) = ((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 0.332 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 0.332 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * sin__q_jB__)) + ( 0.175 * cos__q_jA__));
+ (*this)(1,0) = - cos__q_jA__;
+ (*this)(1,1) = (((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__));
+ (*this)(1,2) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(1,3) = ((((((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 0.332 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 0.332 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * sin__q_jB__)) + ( 0.175 * sin__q_jA__));
+ (*this)(2,1) = (( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__));
+ (*this)(2,2) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(2,3) = ((((((- 0.175 * sin__q_jB__) - ( 0.332 * cos__q_jB__)) * sin__q_jC__) + ((( 0.175 * cos__q_jB__) - ( 0.332 * sin__q_jB__)) * cos__q_jC__)) + ( 1.095 * cos__q_jB__)) + 0.495);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_jE::Type_fr_link0_X_fr_jE()
+{
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_jE& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_jE::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jD__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+
+ (*this)(0,0) = ((( cos__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(0,1) = (((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__));
+ (*this)(0,2) = ((((( cos__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(0,3) = ((((((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 1.271 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__) + (((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * sin__q_jB__)) + ( 0.175 * cos__q_jA__));
+ (*this)(1,0) = ((( sin__q_jA__ * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__));
+ (*this)(1,1) = (( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__));
+ (*this)(1,2) = ((((( sin__q_jA__ * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(1,3) = ((((((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 1.271 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__) + (((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * sin__q_jB__)) + ( 0.175 * sin__q_jA__));
+ (*this)(2,0) = ((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__));
+ (*this)(2,1) = ((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__);
+ (*this)(2,2) = ((( cos__q_jB__ * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ (*this)(2,3) = ((((((- 0.175 * sin__q_jB__) - ( 1.271 * cos__q_jB__)) * sin__q_jC__) + ((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * cos__q_jC__)) + ( 1.095 * cos__q_jB__)) + 0.495);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_jF::Type_fr_link0_X_fr_jF()
+{
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_jF& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link0_X_fr_jF::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR sin__q_jC__;
+ SCALAR sin__q_jE__;
+ SCALAR sin__q_jA__;
+ SCALAR sin__q_jD__;
+ SCALAR cos__q_jA__;
+ SCALAR cos__q_jB__;
+ SCALAR cos__q_jC__;
+ SCALAR cos__q_jD__;
+ SCALAR cos__q_jE__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ sin__q_jA__ = TRAIT::sin( q(JA));
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ cos__q_jA__ = TRAIT::cos( q(JA));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+
+ (*this)(0,0) = ((((( cos__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * cos__q_jE__));
+ (*this)(0,1) = (((((( 1.0 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) + (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) - ( sin__q_jA__ * cos__q_jD__));
+ (*this)(0,2) = (((((((- cos__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - ( sin__q_jA__ * sin__q_jD__)) * sin__q_jE__) + ((((( 1.0 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - (( cos__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(0,3) = ((((((((((((- 0.243 * cos__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.243 * cos__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__) - (( 0.243 * sin__q_jA__) * sin__q_jD__)) * sin__q_jE__) + ((((( 0.243 * cos__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.243 * cos__q_jA__) * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__)) + (((( 0.175 * cos__q_jA__) * cos__q_jB__) - (( 1.271 * cos__q_jA__) * sin__q_jB__)) * sin__q_jC__)) + (((( 0.175 * cos__q_jA__) * sin__q_jB__) + (( 1.271 * cos__q_jA__) * cos__q_jB__)) * cos__q_jC__)) + (( 1.095 * cos__q_jA__) * sin__q_jB__)) + ( 0.175 * cos__q_jA__));
+ (*this)(1,0) = ((((( sin__q_jA__ * sin__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * cos__q_jB__) * cos__q_jC__)) * sin__q_jE__) + ((( cos__q_jA__ * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * cos__q_jE__));
+ (*this)(1,1) = (((((( 1.0 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) + (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * sin__q_jD__) + ( cos__q_jA__ * cos__q_jD__));
+ (*this)(1,2) = ((((( 1.0 * cos__q_jA__) * sin__q_jD__) + ((((- sin__q_jA__ * cos__q_jB__) * sin__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + ((((( 1.0 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - (( sin__q_jA__ * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__));
+ (*this)(1,3) = ((((((((( 0.243 * cos__q_jA__) * sin__q_jD__) + (((((- 0.243 * sin__q_jA__) * cos__q_jB__) * sin__q_jC__) - ((( 0.243 * sin__q_jA__) * sin__q_jB__) * cos__q_jC__)) * cos__q_jD__)) * sin__q_jE__) + ((((( 0.243 * sin__q_jA__) * cos__q_jB__) * cos__q_jC__) - ((( 0.243 * sin__q_jA__) * sin__q_jB__) * sin__q_jC__)) * cos__q_jE__)) + (((( 0.175 * sin__q_jA__) * cos__q_jB__) - (( 1.271 * sin__q_jA__) * sin__q_jB__)) * sin__q_jC__)) + (((( 0.175 * sin__q_jA__) * sin__q_jB__) + (( 1.271 * sin__q_jA__) * cos__q_jB__)) * cos__q_jC__)) + (( 1.095 * sin__q_jA__) * sin__q_jB__)) + ( 0.175 * sin__q_jA__));
+ (*this)(2,0) = (((( cos__q_jB__ * sin__q_jC__) + ( sin__q_jB__ * cos__q_jC__)) * sin__q_jE__) + (((( sin__q_jB__ * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * cos__q_jE__));
+ (*this)(2,1) = (((( 1.0 * cos__q_jB__) * cos__q_jC__) - ( sin__q_jB__ * sin__q_jC__)) * sin__q_jD__);
+ (*this)(2,2) = (((((( 1.0 * sin__q_jB__) * sin__q_jC__) - ( cos__q_jB__ * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + (((- cos__q_jB__ * sin__q_jC__) - ( sin__q_jB__ * cos__q_jC__)) * cos__q_jE__));
+ (*this)(2,3) = (((((((((( 0.243 * sin__q_jB__) * sin__q_jC__) - (( 0.243 * cos__q_jB__) * cos__q_jC__)) * cos__q_jD__) * sin__q_jE__) + ((((- 0.243 * cos__q_jB__) * sin__q_jC__) - (( 0.243 * sin__q_jB__) * cos__q_jC__)) * cos__q_jE__)) + (((- 0.175 * sin__q_jB__) - ( 1.271 * cos__q_jB__)) * sin__q_jC__)) + ((( 0.175 * cos__q_jB__) - ( 1.271 * sin__q_jB__)) * cos__q_jC__)) + ( 1.095 * cos__q_jB__)) + 0.495);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link2_X_fr_link1::Type_fr_link2_X_fr_link1()
+{
+ (*this)(0,1) = 0;
+ (*this)(1,1) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 1.0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link2_X_fr_link1& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link2_X_fr_link1::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR cos__q_jB__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+
+ (*this)(0,0) = sin__q_jB__;
+ (*this)(0,2) = cos__q_jB__;
+ (*this)(0,3) = ((- 0.175 * sin__q_jB__) - ( 0.495 * cos__q_jB__));
+ (*this)(1,0) = cos__q_jB__;
+ (*this)(1,2) = - sin__q_jB__;
+ (*this)(1,3) = (( 0.495 * sin__q_jB__) - ( 0.175 * cos__q_jB__));
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link1_X_fr_link2::Type_fr_link1_X_fr_link2()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0.175;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 0;
+ (*this)(1,2) = 1.0;
+ (*this)(1,3) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0.495;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link1_X_fr_link2& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link1_X_fr_link2::update(const JState& q) {
+ SCALAR sin__q_jB__;
+ SCALAR cos__q_jB__;
+
+ sin__q_jB__ = TRAIT::sin( q(JB));
+ cos__q_jB__ = TRAIT::cos( q(JB));
+
+ (*this)(0,0) = sin__q_jB__;
+ (*this)(0,1) = cos__q_jB__;
+ (*this)(2,0) = cos__q_jB__;
+ (*this)(2,1) = - sin__q_jB__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link3_X_fr_link2::Type_fr_link3_X_fr_link2()
+{
+ (*this)(0,2) = 0;
+ (*this)(1,2) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link3_X_fr_link2& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link3_X_fr_link2::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR cos__q_jC__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+
+ (*this)(0,0) = - sin__q_jC__;
+ (*this)(0,1) = cos__q_jC__;
+ (*this)(0,3) = ( 1.095 * sin__q_jC__);
+ (*this)(1,0) = - cos__q_jC__;
+ (*this)(1,1) = - sin__q_jC__;
+ (*this)(1,3) = ( 1.095 * cos__q_jC__);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link2_X_fr_link3::Type_fr_link2_X_fr_link3()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 1.095;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link2_X_fr_link3& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link2_X_fr_link3::update(const JState& q) {
+ SCALAR sin__q_jC__;
+ SCALAR cos__q_jC__;
+
+ sin__q_jC__ = TRAIT::sin( q(JC));
+ cos__q_jC__ = TRAIT::cos( q(JC));
+
+ (*this)(0,0) = - sin__q_jC__;
+ (*this)(0,1) = - cos__q_jC__;
+ (*this)(1,0) = cos__q_jC__;
+ (*this)(1,1) = - sin__q_jC__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link4_X_fr_link3::Type_fr_link4_X_fr_link3()
+{
+ (*this)(0,0) = 0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = - 0.332;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link4_X_fr_link3& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link4_X_fr_link3::update(const JState& q) {
+ SCALAR sin__q_jD__;
+ SCALAR cos__q_jD__;
+
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+
+ (*this)(0,1) = sin__q_jD__;
+ (*this)(0,2) = - cos__q_jD__;
+ (*this)(0,3) = ( 0.175 * sin__q_jD__);
+ (*this)(1,1) = cos__q_jD__;
+ (*this)(1,2) = sin__q_jD__;
+ (*this)(1,3) = ( 0.175 * cos__q_jD__);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link3_X_fr_link4::Type_fr_link3_X_fr_link4()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 1.0;
+ (*this)(0,3) = 0.332;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = - 0.175;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link3_X_fr_link4& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link3_X_fr_link4::update(const JState& q) {
+ SCALAR sin__q_jD__;
+ SCALAR cos__q_jD__;
+
+ sin__q_jD__ = TRAIT::sin( q(JD));
+ cos__q_jD__ = TRAIT::cos( q(JD));
+
+ (*this)(1,0) = sin__q_jD__;
+ (*this)(1,1) = cos__q_jD__;
+ (*this)(2,0) = - cos__q_jD__;
+ (*this)(2,1) = sin__q_jD__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link5_X_fr_link4::Type_fr_link5_X_fr_link4()
+{
+ (*this)(0,0) = 0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = - 1;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link5_X_fr_link4& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link5_X_fr_link4::update(const JState& q) {
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jE__;
+
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+
+ (*this)(0,1) = sin__q_jE__;
+ (*this)(0,2) = cos__q_jE__;
+ (*this)(0,3) = (- 0.939 * cos__q_jE__);
+ (*this)(1,1) = cos__q_jE__;
+ (*this)(1,2) = - sin__q_jE__;
+ (*this)(1,3) = ( 0.939 * sin__q_jE__);
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link4_X_fr_link5::Type_fr_link4_X_fr_link5()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = - 1;
+ (*this)(0,3) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0.939;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link4_X_fr_link5& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link4_X_fr_link5::update(const JState& q) {
+ SCALAR sin__q_jE__;
+ SCALAR cos__q_jE__;
+
+ sin__q_jE__ = TRAIT::sin( q(JE));
+ cos__q_jE__ = TRAIT::cos( q(JE));
+
+ (*this)(1,0) = sin__q_jE__;
+ (*this)(1,1) = cos__q_jE__;
+ (*this)(2,0) = cos__q_jE__;
+ (*this)(2,1) = - sin__q_jE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link6_X_fr_link5::Type_fr_link6_X_fr_link5()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,3) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,3) = 0;
+ (*this)(2,0) = 1;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = - 0.243;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link6_X_fr_link5& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link6_X_fr_link5::update(const JState& q) {
+ SCALAR sin__q_jF__;
+ SCALAR cos__q_jF__;
+
+ sin__q_jF__ = TRAIT::sin( q(JF));
+ cos__q_jF__ = TRAIT::cos( q(JF));
+
+ (*this)(0,1) = cos__q_jF__;
+ (*this)(0,2) = sin__q_jF__;
+ (*this)(1,1) = - sin__q_jF__;
+ (*this)(1,2) = cos__q_jF__;
+ return *this;
+}
+template <typename TRAIT>
+iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link5_X_fr_link6::Type_fr_link5_X_fr_link6()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 1.0;
+ (*this)(0,3) = 0.243;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+}
+template <typename TRAIT>
+const typename iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link5_X_fr_link6& iit::testirb4600::tpl::HomogeneousTransforms<TRAIT>::Type_fr_link5_X_fr_link6::update(const JState& q) {
+ SCALAR sin__q_jF__;
+ SCALAR cos__q_jF__;
+
+ sin__q_jF__ = TRAIT::sin( q(JF));
+ cos__q_jF__ = TRAIT::cos( q(JF));
+
+ (*this)(1,0) = cos__q_jF__;
+ (*this)(1,1) = - sin__q_jF__;
+ (*this)(2,0) = sin__q_jF__;
+ (*this)(2,1) = cos__q_jF__;
+ return *this;
+}
+
diff --git a/ct_rbd/test/models/testhyq/RobCoGenTestHyQ.h b/ct_rbd/test/models/testhyq/RobCoGenTestHyQ.h
new file mode 100644
index 0000000..4dd7b05
--- /dev/null
+++ b/ct_rbd/test/models/testhyq/RobCoGenTestHyQ.h
@@ -0,0 +1,70 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <Eigen/Core>
+
+#include "generated/declarations.h"
+#include "generated/forward_dynamics.h"
+#include "generated/inertia_properties.h"
+#include "generated/inverse_dynamics.h"
+#include "generated/jacobians.h"
+#include "generated/jsim.h"
+#include "generated/transforms.h"
+#include "generated/link_data_map.h"
+#include "generated/traits.h"
+
+#include <ct/rbd/rbd.h>
+
+
+// these will be undefined later, DO NOT USE!
+
+#define ROBCOGEN_NS TestHyQ // defines the NS of the robot in robcogen, e.g. iit::<ROBCOGEN_NS>
+#define TARGET_NS \
+ TestHyQ // defines the NS where all robot definitions go. Here ct::models::TestHyQ. This defines ct::models::TestHyQ::Dynamics etc.
+
+// define all links of the robot, names as in robcogen
+#define CT_BASE fr_trunk // base link name
+#define CT_L0 fr_LF_hipassembly
+#define CT_L1 fr_LF_upperleg
+#define CT_L2 fr_LF_lowerleg
+#define CT_L3 fr_RF_hipassembly
+#define CT_L4 fr_RF_upperleg
+#define CT_L5 fr_RF_lowerleg
+#define CT_L6 fr_LH_hipassembly
+#define CT_L7 fr_LH_upperleg
+#define CT_L8 fr_LH_lowerleg
+#define CT_L9 fr_RH_hipassembly
+#define CT_L10 fr_RH_upperleg
+#define CT_L11 fr_RH_lowerleg
+
+// number of endeffectors
+#define CT_N_EE 4
+
+// definition of an end-effector
+#define CT_EE0 fr_LF_foot //name of end-effector. Same as frame in RobCoGen
+#define CT_EE0_IS_ON_LINK 3 // to which link is the end-effector rigidly attached to?
+#define CT_EE0_FIRST_JOINT 0 // which is the first joint in the kinematic chain from base to end-effector?
+#define CT_EE0_LAST_JOINT \
+ 2 // which is the last joint in the kinematic chain? THEY NEED TO BE IN ORDER, NO GAPS ALLOWED!
+
+#define CT_EE1 fr_RF_foot
+#define CT_EE1_IS_ON_LINK 6
+#define CT_EE1_FIRST_JOINT 3
+#define CT_EE1_LAST_JOINT 5
+
+#define CT_EE2 fr_LH_foot
+#define CT_EE2_IS_ON_LINK 9
+#define CT_EE2_FIRST_JOINT 6
+#define CT_EE2_LAST_JOINT 8
+
+#define CT_EE3 fr_RH_foot
+#define CT_EE3_IS_ON_LINK 12
+#define CT_EE3_FIRST_JOINT 9
+#define CT_EE3_LAST_JOINT 11
+
+#include <ct/rbd/robot/robcogen/robcogenHelpers.h>
diff --git a/ct_rbd/test/models/testhyq/description/test_hyq.dtdsl b/ct_rbd/test/models/testhyq/description/test_hyq.dtdsl
new file mode 100644
index 0000000..c26898d
--- /dev/null
+++ b/ct_rbd/test/models/testhyq/description/test_hyq.dtdsl
@@ -0,0 +1,90 @@
+Robot TestHyQ
+
+Frames {
+ fr_trunk, fr_LF_foot, fr_RF_foot, fr_LH_foot, fr_RH_foot, trunkCOM,
+ fr_LF_lowerleg, fr_RF_lowerleg, fr_LH_lowerleg, fr_RH_lowerleg,
+ fr_LF_hipassembly, fr_LF_upperleg, fr_LF_lowerleg,
+ fr_RF_hipassembly, fr_RF_upperleg, fr_RF_lowerleg,
+ fr_LH_hipassembly, fr_LH_upperleg, fr_LH_lowerleg,
+ fr_RH_hipassembly, fr_RH_upperleg, fr_RH_lowerleg,
+ LF_hipassemblyCOM, LF_upperlegCOM, LF_lowerlegCOM,
+ RF_hipassemblyCOM, RF_upperlegCOM, RF_lowerlegCOM,
+ LH_hipassemblyCOM, LH_upperlegCOM, LH_lowerlegCOM,
+ RH_hipassemblyCOM, RH_upperlegCOM, RH_lowerlegCOM
+}
+
+Transforms {
+ base=fr_trunk, target=fr_LF_hipassembly
+ base=fr_trunk, target=fr_RF_hipassembly
+ base=fr_trunk, target=fr_LH_hipassembly
+ base=fr_trunk, target=fr_RH_hipassembly
+ base=fr_trunk, target=fr_LF_upperleg
+ base=fr_trunk, target=fr_RF_upperleg
+ base=fr_trunk, target=fr_LH_upperleg
+ base=fr_trunk, target=fr_RH_upperleg
+ base=fr_trunk, target=fr_LF_lowerleg
+ base=fr_trunk, target=fr_RF_lowerleg
+ base=fr_trunk, target=fr_LH_lowerleg
+ base=fr_trunk, target=fr_RH_lowerleg
+
+ base=fr_LF_hipassembly, target=fr_trunk
+ base=fr_RF_hipassembly, target=fr_trunk
+ base=fr_LH_hipassembly, target=fr_trunk
+ base=fr_RH_hipassembly, target=fr_trunk
+ base=fr_LF_upperleg, target=fr_trunk
+ base=fr_RF_upperleg, target=fr_trunk
+ base=fr_LH_upperleg, target=fr_trunk
+ base=fr_RH_upperleg, target=fr_trunk
+ base=fr_LF_lowerleg, target=fr_trunk
+ base=fr_RF_lowerleg, target=fr_trunk
+ base=fr_LH_lowerleg, target=fr_trunk
+ base=fr_RH_lowerleg, target=fr_trunk
+
+ base=fr_trunk, target=LF_hipassemblyCOM
+ base=fr_trunk, target=RF_hipassemblyCOM
+ base=fr_trunk, target=LH_hipassemblyCOM
+ base=fr_trunk, target=RH_hipassemblyCOM
+ base=fr_trunk, target=LF_upperlegCOM
+ base=fr_trunk, target=RF_upperlegCOM
+ base=fr_trunk, target=LH_upperlegCOM
+ base=fr_trunk, target=RH_upperlegCOM
+ base=fr_trunk, target=LF_lowerlegCOM
+ base=fr_trunk, target=RF_lowerlegCOM
+ base=fr_trunk, target=LH_lowerlegCOM
+ base=fr_trunk, target=RH_lowerlegCOM
+ base=fr_LF_lowerleg, target=fr_trunk
+ base=fr_RF_lowerleg, target=fr_trunk
+ base=fr_LH_lowerleg, target=fr_trunk
+ base=fr_RH_lowerleg, target=fr_trunk
+ base=fr_LF_foot, target=fr_LF_lowerleg
+ base=fr_RF_foot, target=fr_RF_lowerleg
+ base=fr_LH_foot, target=fr_LH_lowerleg
+ base=fr_RH_foot, target=fr_RH_lowerleg
+ base=fr_trunk, target=fr_LF_foot
+ base=fr_trunk, target=fr_RF_foot
+ base=fr_trunk, target=fr_LH_foot
+ base=fr_trunk, target=fr_RH_foot
+ base=fr_LF_foot, target=fr_trunk
+ base=fr_RF_foot, target=fr_trunk
+ base=fr_LH_foot, target=fr_trunk
+ base=fr_RH_foot, target=fr_trunk
+}
+
+Jacobians {
+ base=fr_trunk, target=LF_hipassemblyCOM
+ base=fr_trunk, target=RF_hipassemblyCOM
+ base=fr_trunk, target=LH_hipassemblyCOM
+ base=fr_trunk, target=RH_hipassemblyCOM
+ base=fr_trunk, target=LF_upperlegCOM
+ base=fr_trunk, target=RF_upperlegCOM
+ base=fr_trunk, target=LH_upperlegCOM
+ base=fr_trunk, target=RH_upperlegCOM
+ base=fr_trunk, target=LF_lowerlegCOM
+ base=fr_trunk, target=RF_lowerlegCOM
+ base=fr_trunk, target=LH_lowerlegCOM
+ base=fr_trunk, target=RH_lowerlegCOM
+ base=fr_trunk, target=fr_LF_foot
+ base=fr_trunk, target=fr_RF_foot
+ base=fr_trunk, target=fr_LH_foot
+ base=fr_trunk, target=fr_RH_foot
+}
diff --git a/ct_rbd/test/models/testhyq/description/test_hyq.kindsl b/ct_rbd/test/models/testhyq/description/test_hyq.kindsl
new file mode 100644
index 0000000..eca398a
--- /dev/null
+++ b/ct_rbd/test/models/testhyq/description/test_hyq.kindsl
@@ -0,0 +1,420 @@
+Robot TestHyQ {
+RobotBase trunk floating {
+ inertia_params {
+ mass = 47.376
+ CoM = (-0.0223,-0.000, 0.0387)
+ Ix = 1.209488
+ Iy = 5.583700
+ Iz = 6.056973
+ Ixy = 0.005710
+ Ixz = -0.190812
+ Iyz = -0.012668
+ }
+ children {
+ LF_hipassembly via LF_HAA
+ RF_hipassembly via RF_HAA
+ LH_hipassembly via LH_HAA
+ RH_hipassembly via RH_HAA
+ }
+ frames {
+ trunkCOM {
+ translation = (-0.0223,-0.000, 0.0387)
+ rotation = (0.0, 0.0, 0.0)
+ }
+ }
+}
+
+// Left Front leg
+// ---------------
+link LF_hipassembly {
+ id = 1
+ inertia_params {
+ mass = 2.93
+ CoM = (0.043, 0.0, 0.169)
+ Ix = 0.134705
+ Iy = 0.144171
+ Iz = 0.011033
+ Ixy = 0.000036
+ Ixz = 0.022734
+ Iyz = 0.000051
+ }
+ children {
+ LF_upperleg via LF_HFE
+ }
+ frames {
+ LF_hipassemblyCOM {
+ translation = (0.043, 0.0, 0.169)
+ rotation = (0.0, 0.0, 0.0)
+ }
+ }
+}
+
+link LF_upperleg {
+ id = 2
+ inertia_params {
+ mass = 2.638
+ CoM = (0.151, -0.026, -0.0)
+ Ix = 0.005495
+ Iy = 0.087136
+ Iz = 0.089871
+ Ixy = -0.007418
+ Ixz = -0.000102
+ Iyz = -0.000021
+ }
+ children {
+ LF_lowerleg via LF_KFE
+ }
+ frames {
+ LF_upperlegCOM {
+ translation = (0.151, -0.026, -0.0)
+ rotation = (0.0, 0.0, 0.0)
+ }
+ }
+}
+
+link LF_lowerleg {
+ id = 3
+ inertia_params {
+ mass = 0.881
+ CoM = (0.125, 0.000, -0.000)
+ Ix = 0.000468
+ Iy = 0.026409
+ Iz = 0.026181
+ Ixy = 0.0
+ Ixz = 0.0
+ Iyz = 0.0
+ }
+ children {}
+ frames {
+ fr_LF_foot {
+ translation = (0.33, 0.0, 0.0)
+ rotation = (0.0, 0.0, 0.0)
+ }
+ LF_ffs {
+ translation = (0.33, 0.0, 0.0)
+ rotation = (0.0, PI/2.0, -PI/2.0)
+ }
+ LF_lowerlegCOM {
+ translation = (0.125, 0.000, -0.000)
+ rotation = (0.0, 0.0, 0.0)
+ }
+
+ }
+}
+
+
+// Right Front leg
+// ---------------
+link RF_hipassembly {
+ id = 4
+ inertia_params {
+ mass = 2.93
+ CoM = (0.043, -0.0, -0.169)
+ Ix = 0.134705
+ Iy = 0.144171
+ Iz = 0.011033
+ Ixy = -0.000036
+ Ixz = -0.022734
+ Iyz = 0.000051
+ }
+ children {
+ RF_upperleg via RF_HFE
+ }
+ frames {
+ RF_hipassemblyCOM {
+ translation = (0.043, -0.0, -0.169)
+ rotation = (0.0, 0.0, 0.0)
+ }
+ }
+}
+
+link RF_upperleg {
+ id = 5
+ inertia_params {
+ mass = 2.638
+ CoM = (0.151, -0.026, -0.0)
+ Ix = 0.005495
+ Iy = 0.087136
+ Iz = 0.089871
+ Ixy = -0.007418
+ Ixz = -0.000102
+ Iyz = -0.000021 }
+ children {
+ RF_lowerleg via RF_KFE
+ }
+ frames {
+ RF_upperlegCOM {
+ translation = (0.151, -0.026, -0.0)
+ rotation = (0.0, 0.0, 0.0)
+ }
+ }
+}
+
+link RF_lowerleg {
+ id = 6
+ inertia_params {
+ mass = 0.881
+ CoM = (0.125, 0.001, -0.000)
+ Ix = 0.000468
+ Iy = 0.026409
+ Iz = 0.026181
+ Ixy = 0.0
+ Ixz = 0.0
+ Iyz = 0.0
+ }
+ children {}
+ frames {
+ fr_RF_foot {
+ translation = (0.33, 0.0, 0.0)
+ rotation = (0.0, 0.0, 0.0)
+ }
+ RF_ffs {
+ translation = (0.33, 0.0, 0.0)
+ rotation = (0.0, PI/2.0, -PI/2.0)
+ }
+ RF_lowerlegCOM {
+ translation = (0.125, 0.001, -0.000)
+ rotation = (0.0, 0.0, 0.0)
+ }
+ }
+
+}
+
+// Left Hind leg
+// --------------
+link LH_hipassembly {
+ id = 7
+ inertia_params {
+ mass = 2.93
+ CoM = (0.043, -0.0, -0.169)
+ Ix = 0.134705
+ Iy = 0.144171
+ Iz = 0.011033
+ Ixy = -0.000036
+ Ixz = -0.022734
+ Iyz = 0.000051 }
+ children {
+ LH_upperleg via LH_HFE
+ }
+ frames {
+ LH_hipassemblyCOM {
+ translation = (0.043, -0.0, -0.169)
+ rotation = (0.0, 0.0, 0.0)
+ }
+ }
+}
+
+link LH_upperleg {
+ id = 8
+ inertia_params {
+ mass = 2.638
+ CoM = (0.151, 0.026, 0.0)
+ Ix = 0.005495
+ Iy = 0.087136
+ Iz = 0.089871
+ Ixy = 0.007418
+ Ixz = 0.000102
+ Iyz = -0.000021
+ }
+ children {
+ LH_lowerleg via LH_KFE
+ }
+ frames {
+ LH_upperlegCOM {
+ translation = (0.151, 0.026, 0.0)
+ rotation = (0.0, 0.0, 0.0)
+ }
+ }
+}
+
+link LH_lowerleg {
+ id = 9
+ inertia_params {
+ mass = 0.881
+ CoM = (0.125, -0.001, 0.00)
+ Ix = 0.000468
+ Iy = 0.026409
+ Iz = 0.026181
+ Ixy = 0.0
+ Ixz = 0.0
+ Iyz = 0.0
+ }
+ children {}
+ frames {
+ fr_LH_foot {
+ translation = (0.33, 0.0, 0.0)
+ rotation = (0.0, 0.0, 0.0)
+ }
+ LH_ffs {
+ translation = (0.33, 0.0, 0.0)
+ rotation = (0.0, PI/2.0, -PI/2.0)
+ }
+ LH_lowerlegCOM {
+ translation = (0.125, -0.001, 0.00)
+ rotation = (0.0, 0.0, 0.0)
+ }
+ }
+
+}
+
+// Right Hind leg
+// ---------------
+link RH_hipassembly {
+ id = 10
+ inertia_params {
+ mass = 2.93
+ CoM = (0.043, 0.0, 0.169)
+ Ix = 0.134705
+ Iy = 0.144171
+ Iz = 0.011033
+ Ixy = 0.000036
+ Ixz = 0.022734
+ Iyz = 0.000051
+ }
+ children {
+ RH_upperleg via RH_HFE
+ }
+ frames {
+ RH_hipassemblyCOM {
+ translation = (0.043, 0.0, 0.169)
+ rotation = (0.0, 0.0, 0.0)
+ }
+ }
+}
+
+link RH_upperleg {
+ id = 11
+ inertia_params {
+ mass = 2.638
+ CoM = (0.151, 0.026, 0.0)
+ Ix = 0.005495
+ Iy = 0.087136
+ Iz = 0.089871
+ Ixy = 0.007418
+ Ixz = 0.000102
+ Iyz = -0.000021
+ }
+ children {
+ RH_lowerleg via RH_KFE
+ }
+ frames {
+ RH_upperlegCOM {
+ translation = (0.151, 0.026, 0.0)
+ rotation = (0.0, 0.0, 0.0)
+ }
+ }
+}
+
+link RH_lowerleg {
+ id = 12
+ inertia_params {
+ mass = 0.881
+ CoM = (0.125, -0.001, 0.000)
+ Ix = 0.000468
+ Iy = 0.026409
+ Iz = 0.026181
+ Ixy = 0.0
+ Ixz = 0.0
+ Iyz = 0.0
+ }
+ children {}
+ frames {
+ fr_RH_foot {
+ translation = (0.33, 0.0, 0.0)
+ rotation = (0.0, 0.0, 0.0)
+ }
+ RH_ffs {
+ translation = (0.33, 0.0, 0.0)
+ rotation = (0.0, PI/2.0, -PI/2.0)
+ }
+ RH_lowerlegCOM {
+ translation = (0.125, -0.001, 0.000)
+ rotation = (0.0, 0.0, 0.0)
+ }
+ }
+}
+
+
+
+
+r_joint LF_HAA {
+ ref_frame {
+ translation = (0.3735, 0.207, 0.0) //(+DFH/2, +DLR/2, 0)
+ rotation = (0.0, -PI/2.0, PI)
+ }
+}
+r_joint LF_HFE {
+ ref_frame {
+ translation = (0.08, 0.0, 0.0)
+ rotation = (PI/2.0, 0.0, 0.0)
+ }
+}
+r_joint LF_KFE {
+ ref_frame {
+ translation = (0.35, 0.0, 0.0)
+ rotation = (0.0, 0.0, 0.0)
+ }
+}
+
+
+
+r_joint RF_HAA {
+ ref_frame {
+ translation = (0.3735, -0.207, 0.0) //(+DFH/2, -DLR/2, 0)
+ rotation = (0.0, PI/2.0, 0.0) // pi/2 around y
+ }
+}
+r_joint RF_HFE {
+ ref_frame {
+ translation = (0.08, 0.0, 0.0)
+ rotation = (-PI/2.0, 0.0, 0.0)
+ }
+}
+r_joint RF_KFE {
+ ref_frame {
+ translation = (0.35, 0.0, 0.0)
+ rotation = (0.0, 0.0, 0.0)
+ }
+}
+
+r_joint LH_HAA {
+ ref_frame {
+ translation = (-0.3735, 0.207, 0.0) //(-DFH/2, +DLR/2, 0)
+ rotation = (0.0, -PI/2.0, PI)
+ }
+}
+r_joint LH_HFE {
+ ref_frame {
+ translation = (0.08, 0.0, 0.0)
+ rotation = (PI/2.0, 0.0, 0.0)
+ }
+}
+r_joint LH_KFE {
+ ref_frame {
+ translation = (0.35, 0.0, 0.0)
+ rotation = (0.0, 0.0, 0.0)
+ }
+}
+
+
+r_joint RH_HAA {
+ ref_frame {
+ translation = (-0.3735, -0.207, 0.0) //(-DFH/2, -DLR/2, 0)
+ rotation = (0.0, PI/2.0, 0.0)
+ }
+}
+r_joint RH_HFE {
+ ref_frame {
+ translation = (0.08, 0.0, 0.0)
+ rotation = (-PI/2.0, 0.0, 0.0)
+ }
+}
+r_joint RH_KFE {
+ ref_frame {
+ translation = (0.35, 0.0, 0.0)
+ rotation = (0.0, 0.0, 0.0)
+ }
+}
+
+
+}
diff --git a/ct_rbd/test/models/testhyq/generated/declarations.h b/ct_rbd/test/models/testhyq/generated/declarations.h
new file mode 100644
index 0000000..0d3b69e
--- /dev/null
+++ b/ct_rbd/test/models/testhyq/generated/declarations.h
@@ -0,0 +1,64 @@
+#ifndef IIT_ROBOT_TESTHYQ_DECLARATIONS_H_
+#define IIT_ROBOT_TESTHYQ_DECLARATIONS_H_
+
+#include <iit/rbd/rbd.h>
+
+namespace iit {
+namespace TestHyQ {
+
+static const int JointSpaceDimension = 12;
+static const int jointsCount = 12;
+/** The total number of rigid bodies of this robot, including the base */
+static const int linksCount = 13;
+
+namespace tpl {
+template <typename SCALAR>
+using Column12d = iit::rbd::PlainMatrix<SCALAR, 12, 1>;
+
+template <typename SCALAR>
+using JointState = Column12d<SCALAR>;
+}
+
+using Column12d = tpl::Column12d<double>;
+typedef Column12d JointState;
+
+enum JointIdentifiers {
+ LF_HAA = 0
+ , LF_HFE
+ , LF_KFE
+ , RF_HAA
+ , RF_HFE
+ , RF_KFE
+ , LH_HAA
+ , LH_HFE
+ , LH_KFE
+ , RH_HAA
+ , RH_HFE
+ , RH_KFE
+};
+
+enum LinkIdentifiers {
+ TRUNK = 0
+ , LF_HIPASSEMBLY
+ , LF_UPPERLEG
+ , LF_LOWERLEG
+ , RF_HIPASSEMBLY
+ , RF_UPPERLEG
+ , RF_LOWERLEG
+ , LH_HIPASSEMBLY
+ , LH_UPPERLEG
+ , LH_LOWERLEG
+ , RH_HIPASSEMBLY
+ , RH_UPPERLEG
+ , RH_LOWERLEG
+};
+
+static const JointIdentifiers orderedJointIDs[jointsCount] =
+ {LF_HAA,LF_HFE,LF_KFE,RF_HAA,RF_HFE,RF_KFE,LH_HAA,LH_HFE,LH_KFE,RH_HAA,RH_HFE,RH_KFE};
+
+static const LinkIdentifiers orderedLinkIDs[linksCount] =
+ {TRUNK,LF_HIPASSEMBLY,LF_UPPERLEG,LF_LOWERLEG,RF_HIPASSEMBLY,RF_UPPERLEG,RF_LOWERLEG,LH_HIPASSEMBLY,LH_UPPERLEG,LH_LOWERLEG,RH_HIPASSEMBLY,RH_UPPERLEG,RH_LOWERLEG};
+
+}
+}
+#endif
diff --git a/ct_rbd/test/models/testhyq/generated/default_dynparams_getter.h b/ct_rbd/test/models/testhyq/generated/default_dynparams_getter.h
new file mode 100644
index 0000000..e107325
--- /dev/null
+++ b/ct_rbd/test/models/testhyq/generated/default_dynparams_getter.h
@@ -0,0 +1,30 @@
+#ifndef _TESTHYQ_DEFAULT_GETTER_INERTIA_PARAMETERS_
+#define _TESTHYQ_DEFAULT_GETTER_INERTIA_PARAMETERS_
+
+#include "dynamics_parameters.h"
+
+namespace iit {
+namespace TestHyQ {
+namespace dyn {
+
+class DefaultParamsGetter : public RuntimeParamsGetter
+{
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ DefaultParamsGetter() {
+ resetDefaults();
+ }
+ ~DefaultParamsGetter() {};
+
+ public:
+ void resetDefaults() {
+ }
+
+ private:
+ RuntimeInertiaParams values;
+};
+
+}
+}
+}
+#endif
diff --git a/ct_rbd/test/models/testhyq/generated/dynamics_parameters.h b/ct_rbd/test/models/testhyq/generated/dynamics_parameters.h
new file mode 100644
index 0000000..43a3b8b
--- /dev/null
+++ b/ct_rbd/test/models/testhyq/generated/dynamics_parameters.h
@@ -0,0 +1,43 @@
+#ifndef _TESTHYQ_RUNTIME_INERTIA_PARAMETERS_
+#define _TESTHYQ_RUNTIME_INERTIA_PARAMETERS_
+
+namespace iit {
+namespace TestHyQ {
+namespace dyn {
+/**
+ * \defgroup dynparams Dynamics-parameters
+ * Facilities related to the parameters of the inertia properties of the
+ * robot TestHyQ.
+ *
+ * Inertia parameters are non-constants used in the robot model, where the
+ * inertia properties (mass, center of mass, intertia tensor) of the links
+ * are specified. Since the value of such parameters must be resolved
+ * at runtime, we sometimes refer to them as "runtime parameters", "runtime
+ * dynamics parameters", "runtime inertia parameters", etc.
+ *
+ * Do not confuse them with the "inertia properties" of links, which
+ * unfortunately, in the literature, are commonly referred to as
+ * "inertia parameters"... Here, the parameters are the non-constant
+ * fields of the inertia properties.
+ */
+
+ /**
+ * A container for the set of non-constant inertia parameters of the robot TestHyQ
+ * \ingroup dynparams
+ */
+ struct RuntimeInertiaParams {
+ };
+
+ /**
+ * The interface for classes that can compute the actual value of the
+ * non-constant inertia parameters of the robot TestHyQ.
+ * \ingroup dynparams
+ */
+ class RuntimeParamsGetter {
+ public:
+ };
+
+}
+}
+}
+#endif
diff --git a/ct_rbd/test/models/testhyq/generated/forward_dynamics.h b/ct_rbd/test/models/testhyq/generated/forward_dynamics.h
new file mode 100644
index 0000000..33de48f
--- /dev/null
+++ b/ct_rbd/test/models/testhyq/generated/forward_dynamics.h
@@ -0,0 +1,261 @@
+#ifndef IIT_ROBOT_TESTHYQ_FORWARD_DYNAMICS_H_
+#define IIT_ROBOT_TESTHYQ_FORWARD_DYNAMICS_H_
+
+#include <iit/rbd/rbd.h>
+#include <iit/rbd/utils.h>
+#include <iit/rbd/robcogen_commons.h>
+
+#include "declarations.h"
+#include "transforms.h"
+#include "inertia_properties.h"
+#include "link_data_map.h"
+
+namespace iit {
+namespace TestHyQ {
+namespace dyn {
+
+/**
+ * The Forward Dynamics routine for the robot TestHyQ.
+ *
+ * The parameters common to most of the methods are the joint status \c q, the
+ * joint velocities \c qd and the joint forces \c tau. The accelerations \c qdd
+ * will be filled with the computed values. Overloaded methods without the \c q
+ * parameter use the current configuration of the robot; they are provided for
+ * the sake of efficiency, in case the kinematics transforms of the robot have
+ * already been updated elsewhere with the most recent configuration (eg by a
+ * call to setJointStatus()), so that it would be useless to compute them again.
+ */
+
+namespace tpl {
+
+template <typename TRAIT>
+class ForwardDynamics {
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ // Convenient type aliases:
+
+ typedef typename TRAIT::Scalar Scalar;
+
+ typedef iit::rbd::Core<Scalar> CoreS;
+
+ typedef typename CoreS::ForceVector Force;
+ typedef LinkDataMap<Force> ExtForces;
+ typedef typename CoreS::VelocityVector Velocity;
+ typedef typename CoreS::VelocityVector Acceleration;
+ typedef typename CoreS::Column6D Column6DS;
+ typedef typename iit::TestHyQ::tpl::JointState<Scalar> JointState;
+ typedef typename CoreS::Matrix66 Matrix66S;
+ typedef iit::TestHyQ::tpl::MotionTransforms<TRAIT> MTransforms;
+
+public:
+ /**
+ * Default constructor
+ * \param in the inertia properties of the links
+ * \param tr the container of all the spatial motion transforms of
+ * the robot TestHyQ, which will be used by this instance
+ * to compute the dynamics.
+ */
+ ForwardDynamics(iit::TestHyQ::dyn::tpl::InertiaProperties<TRAIT>& in, MTransforms& tr);
+ /** \name Forward dynamics
+ * The Articulated-Body-Algorithm to compute the joint accelerations
+ */ ///@{
+ /**
+ * \param qdd the joint accelerations vector (output parameter).
+ * \param trunk_a
+ * \param trunk_v
+ * \param g the gravity acceleration vector, expressed in the
+ * base coordinates
+ * \param q the joint status vector
+ * \param qd the joint velocities vector
+ * \param tau the joint forces (torque or force)
+ * \param fext the external forces, optional. Each force must be
+ * expressed in the reference frame of the link it is
+ * exerted on.
+ */
+ void fd(
+ JointState& qdd, Acceleration& trunk_a, // output parameters,
+ const Velocity& trunk_v, const Acceleration& g,
+ const JointState& q, const JointState& qd, const JointState& tau, const ExtForces& fext = zeroExtForces);
+ void fd(
+ JointState& qdd, Acceleration& trunk_a, // output parameters,
+ const Velocity& trunk_v, const Acceleration& g,
+ const JointState& qd, const JointState& tau, const ExtForces& fext = zeroExtForces);
+ ///@}
+
+ /** Updates all the kinematics transforms used by this instance. */
+ void setJointStatus(const JointState& q) const;
+
+private:
+ iit::TestHyQ::dyn::tpl::InertiaProperties<TRAIT>* inertiaProps;
+ MTransforms* motionTransforms;
+
+ Matrix66S vcross; // support variable
+ Matrix66S Ia_r; // support variable, articulated inertia in the case of a revolute joint
+ // Link 'trunk'
+ Matrix66S trunk_AI;
+ Force trunk_p;
+
+ // Link 'LF_hipassembly' :
+ Matrix66S LF_hipassembly_AI;
+ Velocity LF_hipassembly_a;
+ Velocity LF_hipassembly_v;
+ Velocity LF_hipassembly_c;
+ Force LF_hipassembly_p;
+
+ Column6DS LF_hipassembly_U;
+ Scalar LF_hipassembly_D;
+ Scalar LF_hipassembly_u;
+ // Link 'LF_upperleg' :
+ Matrix66S LF_upperleg_AI;
+ Velocity LF_upperleg_a;
+ Velocity LF_upperleg_v;
+ Velocity LF_upperleg_c;
+ Force LF_upperleg_p;
+
+ Column6DS LF_upperleg_U;
+ Scalar LF_upperleg_D;
+ Scalar LF_upperleg_u;
+ // Link 'LF_lowerleg' :
+ Matrix66S LF_lowerleg_AI;
+ Velocity LF_lowerleg_a;
+ Velocity LF_lowerleg_v;
+ Velocity LF_lowerleg_c;
+ Force LF_lowerleg_p;
+
+ Column6DS LF_lowerleg_U;
+ Scalar LF_lowerleg_D;
+ Scalar LF_lowerleg_u;
+ // Link 'RF_hipassembly' :
+ Matrix66S RF_hipassembly_AI;
+ Velocity RF_hipassembly_a;
+ Velocity RF_hipassembly_v;
+ Velocity RF_hipassembly_c;
+ Force RF_hipassembly_p;
+
+ Column6DS RF_hipassembly_U;
+ Scalar RF_hipassembly_D;
+ Scalar RF_hipassembly_u;
+ // Link 'RF_upperleg' :
+ Matrix66S RF_upperleg_AI;
+ Velocity RF_upperleg_a;
+ Velocity RF_upperleg_v;
+ Velocity RF_upperleg_c;
+ Force RF_upperleg_p;
+
+ Column6DS RF_upperleg_U;
+ Scalar RF_upperleg_D;
+ Scalar RF_upperleg_u;
+ // Link 'RF_lowerleg' :
+ Matrix66S RF_lowerleg_AI;
+ Velocity RF_lowerleg_a;
+ Velocity RF_lowerleg_v;
+ Velocity RF_lowerleg_c;
+ Force RF_lowerleg_p;
+
+ Column6DS RF_lowerleg_U;
+ Scalar RF_lowerleg_D;
+ Scalar RF_lowerleg_u;
+ // Link 'LH_hipassembly' :
+ Matrix66S LH_hipassembly_AI;
+ Velocity LH_hipassembly_a;
+ Velocity LH_hipassembly_v;
+ Velocity LH_hipassembly_c;
+ Force LH_hipassembly_p;
+
+ Column6DS LH_hipassembly_U;
+ Scalar LH_hipassembly_D;
+ Scalar LH_hipassembly_u;
+ // Link 'LH_upperleg' :
+ Matrix66S LH_upperleg_AI;
+ Velocity LH_upperleg_a;
+ Velocity LH_upperleg_v;
+ Velocity LH_upperleg_c;
+ Force LH_upperleg_p;
+
+ Column6DS LH_upperleg_U;
+ Scalar LH_upperleg_D;
+ Scalar LH_upperleg_u;
+ // Link 'LH_lowerleg' :
+ Matrix66S LH_lowerleg_AI;
+ Velocity LH_lowerleg_a;
+ Velocity LH_lowerleg_v;
+ Velocity LH_lowerleg_c;
+ Force LH_lowerleg_p;
+
+ Column6DS LH_lowerleg_U;
+ Scalar LH_lowerleg_D;
+ Scalar LH_lowerleg_u;
+ // Link 'RH_hipassembly' :
+ Matrix66S RH_hipassembly_AI;
+ Velocity RH_hipassembly_a;
+ Velocity RH_hipassembly_v;
+ Velocity RH_hipassembly_c;
+ Force RH_hipassembly_p;
+
+ Column6DS RH_hipassembly_U;
+ Scalar RH_hipassembly_D;
+ Scalar RH_hipassembly_u;
+ // Link 'RH_upperleg' :
+ Matrix66S RH_upperleg_AI;
+ Velocity RH_upperleg_a;
+ Velocity RH_upperleg_v;
+ Velocity RH_upperleg_c;
+ Force RH_upperleg_p;
+
+ Column6DS RH_upperleg_U;
+ Scalar RH_upperleg_D;
+ Scalar RH_upperleg_u;
+ // Link 'RH_lowerleg' :
+ Matrix66S RH_lowerleg_AI;
+ Velocity RH_lowerleg_a;
+ Velocity RH_lowerleg_v;
+ Velocity RH_lowerleg_c;
+ Force RH_lowerleg_p;
+
+ Column6DS RH_lowerleg_U;
+ Scalar RH_lowerleg_D;
+ Scalar RH_lowerleg_u;
+private:
+ static const ExtForces zeroExtForces;
+};
+
+template <typename TRAIT>
+inline void ForwardDynamics<TRAIT>::setJointStatus(const JointState& q) const {
+ (motionTransforms-> fr_LF_hipassembly_X_fr_trunk)(q);
+ (motionTransforms-> fr_LF_upperleg_X_fr_LF_hipassembly)(q);
+ (motionTransforms-> fr_LF_lowerleg_X_fr_LF_upperleg)(q);
+ (motionTransforms-> fr_RF_hipassembly_X_fr_trunk)(q);
+ (motionTransforms-> fr_RF_upperleg_X_fr_RF_hipassembly)(q);
+ (motionTransforms-> fr_RF_lowerleg_X_fr_RF_upperleg)(q);
+ (motionTransforms-> fr_LH_hipassembly_X_fr_trunk)(q);
+ (motionTransforms-> fr_LH_upperleg_X_fr_LH_hipassembly)(q);
+ (motionTransforms-> fr_LH_lowerleg_X_fr_LH_upperleg)(q);
+ (motionTransforms-> fr_RH_hipassembly_X_fr_trunk)(q);
+ (motionTransforms-> fr_RH_upperleg_X_fr_RH_hipassembly)(q);
+ (motionTransforms-> fr_RH_lowerleg_X_fr_RH_upperleg)(q);
+}
+
+template <typename TRAIT>
+inline void ForwardDynamics<TRAIT>::fd(
+ JointState& qdd, Acceleration& trunk_a, // output parameters,
+ const Velocity& trunk_v, const Acceleration& g,
+ const JointState& q,
+ const JointState& qd,
+ const JointState& tau,
+ const ExtForces& fext/* = zeroExtForces */)
+{
+ setJointStatus(q);
+ fd(qdd, trunk_a, trunk_v, g, qd, tau, fext);
+}
+
+}
+
+typedef tpl::ForwardDynamics<rbd::DoubleTrait> ForwardDynamics;
+
+}
+}
+}
+
+#include "forward_dynamics.impl.h"
+
+#endif
diff --git a/ct_rbd/test/models/testhyq/generated/forward_dynamics.impl.h b/ct_rbd/test/models/testhyq/generated/forward_dynamics.impl.h
new file mode 100644
index 0000000..c689171
--- /dev/null
+++ b/ct_rbd/test/models/testhyq/generated/forward_dynamics.impl.h
@@ -0,0 +1,423 @@
+
+// Initialization of static-const data
+template <typename TRAIT>
+const typename iit::TestHyQ::dyn::tpl::ForwardDynamics<TRAIT>::ExtForces
+ iit::TestHyQ::dyn::tpl::ForwardDynamics<TRAIT>::zeroExtForces(Force::Zero());
+
+template <typename TRAIT>
+iit::TestHyQ::dyn::tpl::ForwardDynamics<TRAIT>::ForwardDynamics(iit::TestHyQ::dyn::tpl::InertiaProperties<TRAIT>& inertia, MTransforms& transforms) :
+ inertiaProps( & inertia ),
+ motionTransforms( & transforms )
+{
+ LF_hipassembly_v.setZero();
+ LF_hipassembly_c.setZero();
+ LF_upperleg_v.setZero();
+ LF_upperleg_c.setZero();
+ LF_lowerleg_v.setZero();
+ LF_lowerleg_c.setZero();
+ RF_hipassembly_v.setZero();
+ RF_hipassembly_c.setZero();
+ RF_upperleg_v.setZero();
+ RF_upperleg_c.setZero();
+ RF_lowerleg_v.setZero();
+ RF_lowerleg_c.setZero();
+ LH_hipassembly_v.setZero();
+ LH_hipassembly_c.setZero();
+ LH_upperleg_v.setZero();
+ LH_upperleg_c.setZero();
+ LH_lowerleg_v.setZero();
+ LH_lowerleg_c.setZero();
+ RH_hipassembly_v.setZero();
+ RH_hipassembly_c.setZero();
+ RH_upperleg_v.setZero();
+ RH_upperleg_c.setZero();
+ RH_lowerleg_v.setZero();
+ RH_lowerleg_c.setZero();
+
+ vcross.setZero();
+ Ia_r.setZero();
+
+}
+
+template <typename TRAIT>
+void iit::TestHyQ::dyn::tpl::ForwardDynamics<TRAIT>::fd(
+ JointState& qdd,
+ Acceleration& trunk_a,
+ const Velocity& trunk_v,
+ const Acceleration& g,
+ const JointState& qd,
+ const JointState& tau,
+ const ExtForces& fext/* = zeroExtForces */)
+{
+
+ trunk_AI = inertiaProps->getTensor_trunk();
+ trunk_p = - fext[TRUNK];
+ LF_hipassembly_AI = inertiaProps->getTensor_LF_hipassembly();
+ LF_hipassembly_p = - fext[LF_HIPASSEMBLY];
+ LF_upperleg_AI = inertiaProps->getTensor_LF_upperleg();
+ LF_upperleg_p = - fext[LF_UPPERLEG];
+ LF_lowerleg_AI = inertiaProps->getTensor_LF_lowerleg();
+ LF_lowerleg_p = - fext[LF_LOWERLEG];
+ RF_hipassembly_AI = inertiaProps->getTensor_RF_hipassembly();
+ RF_hipassembly_p = - fext[RF_HIPASSEMBLY];
+ RF_upperleg_AI = inertiaProps->getTensor_RF_upperleg();
+ RF_upperleg_p = - fext[RF_UPPERLEG];
+ RF_lowerleg_AI = inertiaProps->getTensor_RF_lowerleg();
+ RF_lowerleg_p = - fext[RF_LOWERLEG];
+ LH_hipassembly_AI = inertiaProps->getTensor_LH_hipassembly();
+ LH_hipassembly_p = - fext[LH_HIPASSEMBLY];
+ LH_upperleg_AI = inertiaProps->getTensor_LH_upperleg();
+ LH_upperleg_p = - fext[LH_UPPERLEG];
+ LH_lowerleg_AI = inertiaProps->getTensor_LH_lowerleg();
+ LH_lowerleg_p = - fext[LH_LOWERLEG];
+ RH_hipassembly_AI = inertiaProps->getTensor_RH_hipassembly();
+ RH_hipassembly_p = - fext[RH_HIPASSEMBLY];
+ RH_upperleg_AI = inertiaProps->getTensor_RH_upperleg();
+ RH_upperleg_p = - fext[RH_UPPERLEG];
+ RH_lowerleg_AI = inertiaProps->getTensor_RH_lowerleg();
+ RH_lowerleg_p = - fext[RH_LOWERLEG];
+ // ---------------------- FIRST PASS ---------------------- //
+ // Note that, during the first pass, the articulated inertias are really
+ // just the spatial inertia of the links (see assignments above).
+ // Afterwards things change, and articulated inertias shall not be used
+ // in functions which work specifically with spatial inertias.
+
+ // + Link LF_hipassembly
+ // - The spatial velocity:
+ LF_hipassembly_v = (motionTransforms-> fr_LF_hipassembly_X_fr_trunk) * trunk_v;
+ LF_hipassembly_v(iit::rbd::AZ) += qd(LF_HAA);
+
+ // - The velocity-product acceleration term:
+ iit::rbd::motionCrossProductMx<Scalar>(LF_hipassembly_v, vcross);
+ LF_hipassembly_c = vcross.col(iit::rbd::AZ) * qd(LF_HAA);
+
+ // - The bias force term:
+ LF_hipassembly_p += iit::rbd::vxIv(LF_hipassembly_v, LF_hipassembly_AI);
+
+ // + Link LF_upperleg
+ // - The spatial velocity:
+ LF_upperleg_v = (motionTransforms-> fr_LF_upperleg_X_fr_LF_hipassembly) * LF_hipassembly_v;
+ LF_upperleg_v(iit::rbd::AZ) += qd(LF_HFE);
+
+ // - The velocity-product acceleration term:
+ iit::rbd::motionCrossProductMx<Scalar>(LF_upperleg_v, vcross);
+ LF_upperleg_c = vcross.col(iit::rbd::AZ) * qd(LF_HFE);
+
+ // - The bias force term:
+ LF_upperleg_p += iit::rbd::vxIv(LF_upperleg_v, LF_upperleg_AI);
+
+ // + Link LF_lowerleg
+ // - The spatial velocity:
+ LF_lowerleg_v = (motionTransforms-> fr_LF_lowerleg_X_fr_LF_upperleg) * LF_upperleg_v;
+ LF_lowerleg_v(iit::rbd::AZ) += qd(LF_KFE);
+
+ // - The velocity-product acceleration term:
+ iit::rbd::motionCrossProductMx<Scalar>(LF_lowerleg_v, vcross);
+ LF_lowerleg_c = vcross.col(iit::rbd::AZ) * qd(LF_KFE);
+
+ // - The bias force term:
+ LF_lowerleg_p += iit::rbd::vxIv(LF_lowerleg_v, LF_lowerleg_AI);
+
+ // + Link RF_hipassembly
+ // - The spatial velocity:
+ RF_hipassembly_v = (motionTransforms-> fr_RF_hipassembly_X_fr_trunk) * trunk_v;
+ RF_hipassembly_v(iit::rbd::AZ) += qd(RF_HAA);
+
+ // - The velocity-product acceleration term:
+ iit::rbd::motionCrossProductMx<Scalar>(RF_hipassembly_v, vcross);
+ RF_hipassembly_c = vcross.col(iit::rbd::AZ) * qd(RF_HAA);
+
+ // - The bias force term:
+ RF_hipassembly_p += iit::rbd::vxIv(RF_hipassembly_v, RF_hipassembly_AI);
+
+ // + Link RF_upperleg
+ // - The spatial velocity:
+ RF_upperleg_v = (motionTransforms-> fr_RF_upperleg_X_fr_RF_hipassembly) * RF_hipassembly_v;
+ RF_upperleg_v(iit::rbd::AZ) += qd(RF_HFE);
+
+ // - The velocity-product acceleration term:
+ iit::rbd::motionCrossProductMx<Scalar>(RF_upperleg_v, vcross);
+ RF_upperleg_c = vcross.col(iit::rbd::AZ) * qd(RF_HFE);
+
+ // - The bias force term:
+ RF_upperleg_p += iit::rbd::vxIv(RF_upperleg_v, RF_upperleg_AI);
+
+ // + Link RF_lowerleg
+ // - The spatial velocity:
+ RF_lowerleg_v = (motionTransforms-> fr_RF_lowerleg_X_fr_RF_upperleg) * RF_upperleg_v;
+ RF_lowerleg_v(iit::rbd::AZ) += qd(RF_KFE);
+
+ // - The velocity-product acceleration term:
+ iit::rbd::motionCrossProductMx<Scalar>(RF_lowerleg_v, vcross);
+ RF_lowerleg_c = vcross.col(iit::rbd::AZ) * qd(RF_KFE);
+
+ // - The bias force term:
+ RF_lowerleg_p += iit::rbd::vxIv(RF_lowerleg_v, RF_lowerleg_AI);
+
+ // + Link LH_hipassembly
+ // - The spatial velocity:
+ LH_hipassembly_v = (motionTransforms-> fr_LH_hipassembly_X_fr_trunk) * trunk_v;
+ LH_hipassembly_v(iit::rbd::AZ) += qd(LH_HAA);
+
+ // - The velocity-product acceleration term:
+ iit::rbd::motionCrossProductMx<Scalar>(LH_hipassembly_v, vcross);
+ LH_hipassembly_c = vcross.col(iit::rbd::AZ) * qd(LH_HAA);
+
+ // - The bias force term:
+ LH_hipassembly_p += iit::rbd::vxIv(LH_hipassembly_v, LH_hipassembly_AI);
+
+ // + Link LH_upperleg
+ // - The spatial velocity:
+ LH_upperleg_v = (motionTransforms-> fr_LH_upperleg_X_fr_LH_hipassembly) * LH_hipassembly_v;
+ LH_upperleg_v(iit::rbd::AZ) += qd(LH_HFE);
+
+ // - The velocity-product acceleration term:
+ iit::rbd::motionCrossProductMx<Scalar>(LH_upperleg_v, vcross);
+ LH_upperleg_c = vcross.col(iit::rbd::AZ) * qd(LH_HFE);
+
+ // - The bias force term:
+ LH_upperleg_p += iit::rbd::vxIv(LH_upperleg_v, LH_upperleg_AI);
+
+ // + Link LH_lowerleg
+ // - The spatial velocity:
+ LH_lowerleg_v = (motionTransforms-> fr_LH_lowerleg_X_fr_LH_upperleg) * LH_upperleg_v;
+ LH_lowerleg_v(iit::rbd::AZ) += qd(LH_KFE);
+
+ // - The velocity-product acceleration term:
+ iit::rbd::motionCrossProductMx<Scalar>(LH_lowerleg_v, vcross);
+ LH_lowerleg_c = vcross.col(iit::rbd::AZ) * qd(LH_KFE);
+
+ // - The bias force term:
+ LH_lowerleg_p += iit::rbd::vxIv(LH_lowerleg_v, LH_lowerleg_AI);
+
+ // + Link RH_hipassembly
+ // - The spatial velocity:
+ RH_hipassembly_v = (motionTransforms-> fr_RH_hipassembly_X_fr_trunk) * trunk_v;
+ RH_hipassembly_v(iit::rbd::AZ) += qd(RH_HAA);
+
+ // - The velocity-product acceleration term:
+ iit::rbd::motionCrossProductMx<Scalar>(RH_hipassembly_v, vcross);
+ RH_hipassembly_c = vcross.col(iit::rbd::AZ) * qd(RH_HAA);
+
+ // - The bias force term:
+ RH_hipassembly_p += iit::rbd::vxIv(RH_hipassembly_v, RH_hipassembly_AI);
+
+ // + Link RH_upperleg
+ // - The spatial velocity:
+ RH_upperleg_v = (motionTransforms-> fr_RH_upperleg_X_fr_RH_hipassembly) * RH_hipassembly_v;
+ RH_upperleg_v(iit::rbd::AZ) += qd(RH_HFE);
+
+ // - The velocity-product acceleration term:
+ iit::rbd::motionCrossProductMx<Scalar>(RH_upperleg_v, vcross);
+ RH_upperleg_c = vcross.col(iit::rbd::AZ) * qd(RH_HFE);
+
+ // - The bias force term:
+ RH_upperleg_p += iit::rbd::vxIv(RH_upperleg_v, RH_upperleg_AI);
+
+ // + Link RH_lowerleg
+ // - The spatial velocity:
+ RH_lowerleg_v = (motionTransforms-> fr_RH_lowerleg_X_fr_RH_upperleg) * RH_upperleg_v;
+ RH_lowerleg_v(iit::rbd::AZ) += qd(RH_KFE);
+
+ // - The velocity-product acceleration term:
+ iit::rbd::motionCrossProductMx<Scalar>(RH_lowerleg_v, vcross);
+ RH_lowerleg_c = vcross.col(iit::rbd::AZ) * qd(RH_KFE);
+
+ // - The bias force term:
+ RH_lowerleg_p += iit::rbd::vxIv(RH_lowerleg_v, RH_lowerleg_AI);
+
+ // + The floating base body
+ trunk_p += iit::rbd::vxIv(trunk_v, trunk_AI);
+
+ // ---------------------- SECOND PASS ---------------------- //
+ Matrix66S IaB;
+ Force pa;
+
+ // + Link RH_lowerleg
+ RH_lowerleg_u = tau(RH_KFE) - RH_lowerleg_p(iit::rbd::AZ);
+ RH_lowerleg_U = RH_lowerleg_AI.col(iit::rbd::AZ);
+ RH_lowerleg_D = RH_lowerleg_U(iit::rbd::AZ);
+
+ iit::rbd::compute_Ia_revolute(RH_lowerleg_AI, RH_lowerleg_U, RH_lowerleg_D, Ia_r); // same as: Ia_r = RH_lowerleg_AI - RH_lowerleg_U/RH_lowerleg_D * RH_lowerleg_U.transpose();
+ pa = RH_lowerleg_p + Ia_r * RH_lowerleg_c + RH_lowerleg_U * RH_lowerleg_u/RH_lowerleg_D;
+ ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_RH_lowerleg_X_fr_RH_upperleg, IaB);
+ RH_upperleg_AI += IaB;
+ RH_upperleg_p += (motionTransforms-> fr_RH_lowerleg_X_fr_RH_upperleg).transpose() * pa;
+
+ // + Link RH_upperleg
+ RH_upperleg_u = tau(RH_HFE) - RH_upperleg_p(iit::rbd::AZ);
+ RH_upperleg_U = RH_upperleg_AI.col(iit::rbd::AZ);
+ RH_upperleg_D = RH_upperleg_U(iit::rbd::AZ);
+
+ iit::rbd::compute_Ia_revolute(RH_upperleg_AI, RH_upperleg_U, RH_upperleg_D, Ia_r); // same as: Ia_r = RH_upperleg_AI - RH_upperleg_U/RH_upperleg_D * RH_upperleg_U.transpose();
+ pa = RH_upperleg_p + Ia_r * RH_upperleg_c + RH_upperleg_U * RH_upperleg_u/RH_upperleg_D;
+ ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_RH_upperleg_X_fr_RH_hipassembly, IaB);
+ RH_hipassembly_AI += IaB;
+ RH_hipassembly_p += (motionTransforms-> fr_RH_upperleg_X_fr_RH_hipassembly).transpose() * pa;
+
+ // + Link RH_hipassembly
+ RH_hipassembly_u = tau(RH_HAA) - RH_hipassembly_p(iit::rbd::AZ);
+ RH_hipassembly_U = RH_hipassembly_AI.col(iit::rbd::AZ);
+ RH_hipassembly_D = RH_hipassembly_U(iit::rbd::AZ);
+
+ iit::rbd::compute_Ia_revolute(RH_hipassembly_AI, RH_hipassembly_U, RH_hipassembly_D, Ia_r); // same as: Ia_r = RH_hipassembly_AI - RH_hipassembly_U/RH_hipassembly_D * RH_hipassembly_U.transpose();
+ pa = RH_hipassembly_p + Ia_r * RH_hipassembly_c + RH_hipassembly_U * RH_hipassembly_u/RH_hipassembly_D;
+ ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_RH_hipassembly_X_fr_trunk, IaB);
+ trunk_AI += IaB;
+ trunk_p += (motionTransforms-> fr_RH_hipassembly_X_fr_trunk).transpose() * pa;
+
+ // + Link LH_lowerleg
+ LH_lowerleg_u = tau(LH_KFE) - LH_lowerleg_p(iit::rbd::AZ);
+ LH_lowerleg_U = LH_lowerleg_AI.col(iit::rbd::AZ);
+ LH_lowerleg_D = LH_lowerleg_U(iit::rbd::AZ);
+
+ iit::rbd::compute_Ia_revolute(LH_lowerleg_AI, LH_lowerleg_U, LH_lowerleg_D, Ia_r); // same as: Ia_r = LH_lowerleg_AI - LH_lowerleg_U/LH_lowerleg_D * LH_lowerleg_U.transpose();
+ pa = LH_lowerleg_p + Ia_r * LH_lowerleg_c + LH_lowerleg_U * LH_lowerleg_u/LH_lowerleg_D;
+ ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_LH_lowerleg_X_fr_LH_upperleg, IaB);
+ LH_upperleg_AI += IaB;
+ LH_upperleg_p += (motionTransforms-> fr_LH_lowerleg_X_fr_LH_upperleg).transpose() * pa;
+
+ // + Link LH_upperleg
+ LH_upperleg_u = tau(LH_HFE) - LH_upperleg_p(iit::rbd::AZ);
+ LH_upperleg_U = LH_upperleg_AI.col(iit::rbd::AZ);
+ LH_upperleg_D = LH_upperleg_U(iit::rbd::AZ);
+
+ iit::rbd::compute_Ia_revolute(LH_upperleg_AI, LH_upperleg_U, LH_upperleg_D, Ia_r); // same as: Ia_r = LH_upperleg_AI - LH_upperleg_U/LH_upperleg_D * LH_upperleg_U.transpose();
+ pa = LH_upperleg_p + Ia_r * LH_upperleg_c + LH_upperleg_U * LH_upperleg_u/LH_upperleg_D;
+ ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_LH_upperleg_X_fr_LH_hipassembly, IaB);
+ LH_hipassembly_AI += IaB;
+ LH_hipassembly_p += (motionTransforms-> fr_LH_upperleg_X_fr_LH_hipassembly).transpose() * pa;
+
+ // + Link LH_hipassembly
+ LH_hipassembly_u = tau(LH_HAA) - LH_hipassembly_p(iit::rbd::AZ);
+ LH_hipassembly_U = LH_hipassembly_AI.col(iit::rbd::AZ);
+ LH_hipassembly_D = LH_hipassembly_U(iit::rbd::AZ);
+
+ iit::rbd::compute_Ia_revolute(LH_hipassembly_AI, LH_hipassembly_U, LH_hipassembly_D, Ia_r); // same as: Ia_r = LH_hipassembly_AI - LH_hipassembly_U/LH_hipassembly_D * LH_hipassembly_U.transpose();
+ pa = LH_hipassembly_p + Ia_r * LH_hipassembly_c + LH_hipassembly_U * LH_hipassembly_u/LH_hipassembly_D;
+ ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_LH_hipassembly_X_fr_trunk, IaB);
+ trunk_AI += IaB;
+ trunk_p += (motionTransforms-> fr_LH_hipassembly_X_fr_trunk).transpose() * pa;
+
+ // + Link RF_lowerleg
+ RF_lowerleg_u = tau(RF_KFE) - RF_lowerleg_p(iit::rbd::AZ);
+ RF_lowerleg_U = RF_lowerleg_AI.col(iit::rbd::AZ);
+ RF_lowerleg_D = RF_lowerleg_U(iit::rbd::AZ);
+
+ iit::rbd::compute_Ia_revolute(RF_lowerleg_AI, RF_lowerleg_U, RF_lowerleg_D, Ia_r); // same as: Ia_r = RF_lowerleg_AI - RF_lowerleg_U/RF_lowerleg_D * RF_lowerleg_U.transpose();
+ pa = RF_lowerleg_p + Ia_r * RF_lowerleg_c + RF_lowerleg_U * RF_lowerleg_u/RF_lowerleg_D;
+ ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_RF_lowerleg_X_fr_RF_upperleg, IaB);
+ RF_upperleg_AI += IaB;
+ RF_upperleg_p += (motionTransforms-> fr_RF_lowerleg_X_fr_RF_upperleg).transpose() * pa;
+
+ // + Link RF_upperleg
+ RF_upperleg_u = tau(RF_HFE) - RF_upperleg_p(iit::rbd::AZ);
+ RF_upperleg_U = RF_upperleg_AI.col(iit::rbd::AZ);
+ RF_upperleg_D = RF_upperleg_U(iit::rbd::AZ);
+
+ iit::rbd::compute_Ia_revolute(RF_upperleg_AI, RF_upperleg_U, RF_upperleg_D, Ia_r); // same as: Ia_r = RF_upperleg_AI - RF_upperleg_U/RF_upperleg_D * RF_upperleg_U.transpose();
+ pa = RF_upperleg_p + Ia_r * RF_upperleg_c + RF_upperleg_U * RF_upperleg_u/RF_upperleg_D;
+ ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_RF_upperleg_X_fr_RF_hipassembly, IaB);
+ RF_hipassembly_AI += IaB;
+ RF_hipassembly_p += (motionTransforms-> fr_RF_upperleg_X_fr_RF_hipassembly).transpose() * pa;
+
+ // + Link RF_hipassembly
+ RF_hipassembly_u = tau(RF_HAA) - RF_hipassembly_p(iit::rbd::AZ);
+ RF_hipassembly_U = RF_hipassembly_AI.col(iit::rbd::AZ);
+ RF_hipassembly_D = RF_hipassembly_U(iit::rbd::AZ);
+
+ iit::rbd::compute_Ia_revolute(RF_hipassembly_AI, RF_hipassembly_U, RF_hipassembly_D, Ia_r); // same as: Ia_r = RF_hipassembly_AI - RF_hipassembly_U/RF_hipassembly_D * RF_hipassembly_U.transpose();
+ pa = RF_hipassembly_p + Ia_r * RF_hipassembly_c + RF_hipassembly_U * RF_hipassembly_u/RF_hipassembly_D;
+ ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_RF_hipassembly_X_fr_trunk, IaB);
+ trunk_AI += IaB;
+ trunk_p += (motionTransforms-> fr_RF_hipassembly_X_fr_trunk).transpose() * pa;
+
+ // + Link LF_lowerleg
+ LF_lowerleg_u = tau(LF_KFE) - LF_lowerleg_p(iit::rbd::AZ);
+ LF_lowerleg_U = LF_lowerleg_AI.col(iit::rbd::AZ);
+ LF_lowerleg_D = LF_lowerleg_U(iit::rbd::AZ);
+
+ iit::rbd::compute_Ia_revolute(LF_lowerleg_AI, LF_lowerleg_U, LF_lowerleg_D, Ia_r); // same as: Ia_r = LF_lowerleg_AI - LF_lowerleg_U/LF_lowerleg_D * LF_lowerleg_U.transpose();
+ pa = LF_lowerleg_p + Ia_r * LF_lowerleg_c + LF_lowerleg_U * LF_lowerleg_u/LF_lowerleg_D;
+ ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_LF_lowerleg_X_fr_LF_upperleg, IaB);
+ LF_upperleg_AI += IaB;
+ LF_upperleg_p += (motionTransforms-> fr_LF_lowerleg_X_fr_LF_upperleg).transpose() * pa;
+
+ // + Link LF_upperleg
+ LF_upperleg_u = tau(LF_HFE) - LF_upperleg_p(iit::rbd::AZ);
+ LF_upperleg_U = LF_upperleg_AI.col(iit::rbd::AZ);
+ LF_upperleg_D = LF_upperleg_U(iit::rbd::AZ);
+
+ iit::rbd::compute_Ia_revolute(LF_upperleg_AI, LF_upperleg_U, LF_upperleg_D, Ia_r); // same as: Ia_r = LF_upperleg_AI - LF_upperleg_U/LF_upperleg_D * LF_upperleg_U.transpose();
+ pa = LF_upperleg_p + Ia_r * LF_upperleg_c + LF_upperleg_U * LF_upperleg_u/LF_upperleg_D;
+ ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_LF_upperleg_X_fr_LF_hipassembly, IaB);
+ LF_hipassembly_AI += IaB;
+ LF_hipassembly_p += (motionTransforms-> fr_LF_upperleg_X_fr_LF_hipassembly).transpose() * pa;
+
+ // + Link LF_hipassembly
+ LF_hipassembly_u = tau(LF_HAA) - LF_hipassembly_p(iit::rbd::AZ);
+ LF_hipassembly_U = LF_hipassembly_AI.col(iit::rbd::AZ);
+ LF_hipassembly_D = LF_hipassembly_U(iit::rbd::AZ);
+
+ iit::rbd::compute_Ia_revolute(LF_hipassembly_AI, LF_hipassembly_U, LF_hipassembly_D, Ia_r); // same as: Ia_r = LF_hipassembly_AI - LF_hipassembly_U/LF_hipassembly_D * LF_hipassembly_U.transpose();
+ pa = LF_hipassembly_p + Ia_r * LF_hipassembly_c + LF_hipassembly_U * LF_hipassembly_u/LF_hipassembly_D;
+ ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_LF_hipassembly_X_fr_trunk, IaB);
+ trunk_AI += IaB;
+ trunk_p += (motionTransforms-> fr_LF_hipassembly_X_fr_trunk).transpose() * pa;
+
+ // + The acceleration of the floating base trunk, without gravity
+ trunk_a = - TRAIT::solve(trunk_AI, trunk_p); // trunk_a = - IA^-1 * trunk_p
+
+ // ---------------------- THIRD PASS ---------------------- //
+ LF_hipassembly_a = (motionTransforms-> fr_LF_hipassembly_X_fr_trunk) * trunk_a + LF_hipassembly_c;
+ qdd(LF_HAA) = (LF_hipassembly_u - LF_hipassembly_U.dot(LF_hipassembly_a)) / LF_hipassembly_D;
+ LF_hipassembly_a(iit::rbd::AZ) += qdd(LF_HAA);
+
+ LF_upperleg_a = (motionTransforms-> fr_LF_upperleg_X_fr_LF_hipassembly) * LF_hipassembly_a + LF_upperleg_c;
+ qdd(LF_HFE) = (LF_upperleg_u - LF_upperleg_U.dot(LF_upperleg_a)) / LF_upperleg_D;
+ LF_upperleg_a(iit::rbd::AZ) += qdd(LF_HFE);
+
+ LF_lowerleg_a = (motionTransforms-> fr_LF_lowerleg_X_fr_LF_upperleg) * LF_upperleg_a + LF_lowerleg_c;
+ qdd(LF_KFE) = (LF_lowerleg_u - LF_lowerleg_U.dot(LF_lowerleg_a)) / LF_lowerleg_D;
+ LF_lowerleg_a(iit::rbd::AZ) += qdd(LF_KFE);
+
+ RF_hipassembly_a = (motionTransforms-> fr_RF_hipassembly_X_fr_trunk) * trunk_a + RF_hipassembly_c;
+ qdd(RF_HAA) = (RF_hipassembly_u - RF_hipassembly_U.dot(RF_hipassembly_a)) / RF_hipassembly_D;
+ RF_hipassembly_a(iit::rbd::AZ) += qdd(RF_HAA);
+
+ RF_upperleg_a = (motionTransforms-> fr_RF_upperleg_X_fr_RF_hipassembly) * RF_hipassembly_a + RF_upperleg_c;
+ qdd(RF_HFE) = (RF_upperleg_u - RF_upperleg_U.dot(RF_upperleg_a)) / RF_upperleg_D;
+ RF_upperleg_a(iit::rbd::AZ) += qdd(RF_HFE);
+
+ RF_lowerleg_a = (motionTransforms-> fr_RF_lowerleg_X_fr_RF_upperleg) * RF_upperleg_a + RF_lowerleg_c;
+ qdd(RF_KFE) = (RF_lowerleg_u - RF_lowerleg_U.dot(RF_lowerleg_a)) / RF_lowerleg_D;
+ RF_lowerleg_a(iit::rbd::AZ) += qdd(RF_KFE);
+
+ LH_hipassembly_a = (motionTransforms-> fr_LH_hipassembly_X_fr_trunk) * trunk_a + LH_hipassembly_c;
+ qdd(LH_HAA) = (LH_hipassembly_u - LH_hipassembly_U.dot(LH_hipassembly_a)) / LH_hipassembly_D;
+ LH_hipassembly_a(iit::rbd::AZ) += qdd(LH_HAA);
+
+ LH_upperleg_a = (motionTransforms-> fr_LH_upperleg_X_fr_LH_hipassembly) * LH_hipassembly_a + LH_upperleg_c;
+ qdd(LH_HFE) = (LH_upperleg_u - LH_upperleg_U.dot(LH_upperleg_a)) / LH_upperleg_D;
+ LH_upperleg_a(iit::rbd::AZ) += qdd(LH_HFE);
+
+ LH_lowerleg_a = (motionTransforms-> fr_LH_lowerleg_X_fr_LH_upperleg) * LH_upperleg_a + LH_lowerleg_c;
+ qdd(LH_KFE) = (LH_lowerleg_u - LH_lowerleg_U.dot(LH_lowerleg_a)) / LH_lowerleg_D;
+ LH_lowerleg_a(iit::rbd::AZ) += qdd(LH_KFE);
+
+ RH_hipassembly_a = (motionTransforms-> fr_RH_hipassembly_X_fr_trunk) * trunk_a + RH_hipassembly_c;
+ qdd(RH_HAA) = (RH_hipassembly_u - RH_hipassembly_U.dot(RH_hipassembly_a)) / RH_hipassembly_D;
+ RH_hipassembly_a(iit::rbd::AZ) += qdd(RH_HAA);
+
+ RH_upperleg_a = (motionTransforms-> fr_RH_upperleg_X_fr_RH_hipassembly) * RH_hipassembly_a + RH_upperleg_c;
+ qdd(RH_HFE) = (RH_upperleg_u - RH_upperleg_U.dot(RH_upperleg_a)) / RH_upperleg_D;
+ RH_upperleg_a(iit::rbd::AZ) += qdd(RH_HFE);
+
+ RH_lowerleg_a = (motionTransforms-> fr_RH_lowerleg_X_fr_RH_upperleg) * RH_upperleg_a + RH_lowerleg_c;
+ qdd(RH_KFE) = (RH_lowerleg_u - RH_lowerleg_U.dot(RH_lowerleg_a)) / RH_lowerleg_D;
+ RH_lowerleg_a(iit::rbd::AZ) += qdd(RH_KFE);
+
+
+ // + Add gravity to the acceleration of the floating base
+ trunk_a += g;
+}
diff --git a/ct_rbd/test/models/testhyq/generated/inertia_properties.h b/ct_rbd/test/models/testhyq/generated/inertia_properties.h
new file mode 100644
index 0000000..4fc1928
--- /dev/null
+++ b/ct_rbd/test/models/testhyq/generated/inertia_properties.h
@@ -0,0 +1,281 @@
+#ifndef IIT_ROBOT_TESTHYQ_INERTIA_PROPERTIES_H_
+#define IIT_ROBOT_TESTHYQ_INERTIA_PROPERTIES_H_
+
+#include <iit/rbd/rbd.h>
+#include <iit/rbd/InertiaMatrix.h>
+#include <iit/rbd/utils.h>
+#include <iit/rbd/traits/DoubleTrait.h>
+
+#include "declarations.h"
+
+namespace iit {
+namespace TestHyQ {
+/**
+ * This namespace encloses classes and functions related to the Dynamics
+ * of the robot TestHyQ.
+ */
+namespace dyn {
+
+using InertiaMatrix = iit::rbd::InertiaMatrixDense;
+
+namespace tpl {
+
+template <typename TRAIT>
+class InertiaProperties {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef typename TRAIT::Scalar Scalar;
+ typedef iit::rbd::Core<Scalar> CoreS;
+ typedef iit::rbd::tpl::InertiaMatrixDense<Scalar> IMatrix;
+ typedef typename CoreS::Vector3 Vec3d;
+
+ InertiaProperties();
+ ~InertiaProperties();
+ const IMatrix& getTensor_trunk() const;
+ const IMatrix& getTensor_LF_hipassembly() const;
+ const IMatrix& getTensor_LF_upperleg() const;
+ const IMatrix& getTensor_LF_lowerleg() const;
+ const IMatrix& getTensor_RF_hipassembly() const;
+ const IMatrix& getTensor_RF_upperleg() const;
+ const IMatrix& getTensor_RF_lowerleg() const;
+ const IMatrix& getTensor_LH_hipassembly() const;
+ const IMatrix& getTensor_LH_upperleg() const;
+ const IMatrix& getTensor_LH_lowerleg() const;
+ const IMatrix& getTensor_RH_hipassembly() const;
+ const IMatrix& getTensor_RH_upperleg() const;
+ const IMatrix& getTensor_RH_lowerleg() const;
+ Scalar getMass_trunk() const;
+ Scalar getMass_LF_hipassembly() const;
+ Scalar getMass_LF_upperleg() const;
+ Scalar getMass_LF_lowerleg() const;
+ Scalar getMass_RF_hipassembly() const;
+ Scalar getMass_RF_upperleg() const;
+ Scalar getMass_RF_lowerleg() const;
+ Scalar getMass_LH_hipassembly() const;
+ Scalar getMass_LH_upperleg() const;
+ Scalar getMass_LH_lowerleg() const;
+ Scalar getMass_RH_hipassembly() const;
+ Scalar getMass_RH_upperleg() const;
+ Scalar getMass_RH_lowerleg() const;
+ const Vec3d& getCOM_trunk() const;
+ const Vec3d& getCOM_LF_hipassembly() const;
+ const Vec3d& getCOM_LF_upperleg() const;
+ const Vec3d& getCOM_LF_lowerleg() const;
+ const Vec3d& getCOM_RF_hipassembly() const;
+ const Vec3d& getCOM_RF_upperleg() const;
+ const Vec3d& getCOM_RF_lowerleg() const;
+ const Vec3d& getCOM_LH_hipassembly() const;
+ const Vec3d& getCOM_LH_upperleg() const;
+ const Vec3d& getCOM_LH_lowerleg() const;
+ const Vec3d& getCOM_RH_hipassembly() const;
+ const Vec3d& getCOM_RH_upperleg() const;
+ const Vec3d& getCOM_RH_lowerleg() const;
+ Scalar getTotalMass() const;
+
+ private:
+
+ IMatrix tensor_trunk;
+ IMatrix tensor_LF_hipassembly;
+ IMatrix tensor_LF_upperleg;
+ IMatrix tensor_LF_lowerleg;
+ IMatrix tensor_RF_hipassembly;
+ IMatrix tensor_RF_upperleg;
+ IMatrix tensor_RF_lowerleg;
+ IMatrix tensor_LH_hipassembly;
+ IMatrix tensor_LH_upperleg;
+ IMatrix tensor_LH_lowerleg;
+ IMatrix tensor_RH_hipassembly;
+ IMatrix tensor_RH_upperleg;
+ IMatrix tensor_RH_lowerleg;
+ Vec3d com_trunk;
+ Vec3d com_LF_hipassembly;
+ Vec3d com_LF_upperleg;
+ Vec3d com_LF_lowerleg;
+ Vec3d com_RF_hipassembly;
+ Vec3d com_RF_upperleg;
+ Vec3d com_RF_lowerleg;
+ Vec3d com_LH_hipassembly;
+ Vec3d com_LH_upperleg;
+ Vec3d com_LH_lowerleg;
+ Vec3d com_RH_hipassembly;
+ Vec3d com_RH_upperleg;
+ Vec3d com_RH_lowerleg;
+};
+
+template <typename TRAIT>
+inline InertiaProperties<TRAIT>::~InertiaProperties() {}
+
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::IMatrix& InertiaProperties<TRAIT>::getTensor_trunk() const {
+ return this->tensor_trunk;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::IMatrix& InertiaProperties<TRAIT>::getTensor_LF_hipassembly() const {
+ return this->tensor_LF_hipassembly;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::IMatrix& InertiaProperties<TRAIT>::getTensor_LF_upperleg() const {
+ return this->tensor_LF_upperleg;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::IMatrix& InertiaProperties<TRAIT>::getTensor_LF_lowerleg() const {
+ return this->tensor_LF_lowerleg;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::IMatrix& InertiaProperties<TRAIT>::getTensor_RF_hipassembly() const {
+ return this->tensor_RF_hipassembly;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::IMatrix& InertiaProperties<TRAIT>::getTensor_RF_upperleg() const {
+ return this->tensor_RF_upperleg;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::IMatrix& InertiaProperties<TRAIT>::getTensor_RF_lowerleg() const {
+ return this->tensor_RF_lowerleg;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::IMatrix& InertiaProperties<TRAIT>::getTensor_LH_hipassembly() const {
+ return this->tensor_LH_hipassembly;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::IMatrix& InertiaProperties<TRAIT>::getTensor_LH_upperleg() const {
+ return this->tensor_LH_upperleg;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::IMatrix& InertiaProperties<TRAIT>::getTensor_LH_lowerleg() const {
+ return this->tensor_LH_lowerleg;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::IMatrix& InertiaProperties<TRAIT>::getTensor_RH_hipassembly() const {
+ return this->tensor_RH_hipassembly;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::IMatrix& InertiaProperties<TRAIT>::getTensor_RH_upperleg() const {
+ return this->tensor_RH_upperleg;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::IMatrix& InertiaProperties<TRAIT>::getTensor_RH_lowerleg() const {
+ return this->tensor_RH_lowerleg;
+}
+template <typename TRAIT>
+inline typename InertiaProperties<TRAIT>::Scalar InertiaProperties<TRAIT>::getMass_trunk() const {
+ return this->tensor_trunk.getMass();
+}
+template <typename TRAIT>
+inline typename InertiaProperties<TRAIT>::Scalar InertiaProperties<TRAIT>::getMass_LF_hipassembly() const {
+ return this->tensor_LF_hipassembly.getMass();
+}
+template <typename TRAIT>
+inline typename InertiaProperties<TRAIT>::Scalar InertiaProperties<TRAIT>::getMass_LF_upperleg() const {
+ return this->tensor_LF_upperleg.getMass();
+}
+template <typename TRAIT>
+inline typename InertiaProperties<TRAIT>::Scalar InertiaProperties<TRAIT>::getMass_LF_lowerleg() const {
+ return this->tensor_LF_lowerleg.getMass();
+}
+template <typename TRAIT>
+inline typename InertiaProperties<TRAIT>::Scalar InertiaProperties<TRAIT>::getMass_RF_hipassembly() const {
+ return this->tensor_RF_hipassembly.getMass();
+}
+template <typename TRAIT>
+inline typename InertiaProperties<TRAIT>::Scalar InertiaProperties<TRAIT>::getMass_RF_upperleg() const {
+ return this->tensor_RF_upperleg.getMass();
+}
+template <typename TRAIT>
+inline typename InertiaProperties<TRAIT>::Scalar InertiaProperties<TRAIT>::getMass_RF_lowerleg() const {
+ return this->tensor_RF_lowerleg.getMass();
+}
+template <typename TRAIT>
+inline typename InertiaProperties<TRAIT>::Scalar InertiaProperties<TRAIT>::getMass_LH_hipassembly() const {
+ return this->tensor_LH_hipassembly.getMass();
+}
+template <typename TRAIT>
+inline typename InertiaProperties<TRAIT>::Scalar InertiaProperties<TRAIT>::getMass_LH_upperleg() const {
+ return this->tensor_LH_upperleg.getMass();
+}
+template <typename TRAIT>
+inline typename InertiaProperties<TRAIT>::Scalar InertiaProperties<TRAIT>::getMass_LH_lowerleg() const {
+ return this->tensor_LH_lowerleg.getMass();
+}
+template <typename TRAIT>
+inline typename InertiaProperties<TRAIT>::Scalar InertiaProperties<TRAIT>::getMass_RH_hipassembly() const {
+ return this->tensor_RH_hipassembly.getMass();
+}
+template <typename TRAIT>
+inline typename InertiaProperties<TRAIT>::Scalar InertiaProperties<TRAIT>::getMass_RH_upperleg() const {
+ return this->tensor_RH_upperleg.getMass();
+}
+template <typename TRAIT>
+inline typename InertiaProperties<TRAIT>::Scalar InertiaProperties<TRAIT>::getMass_RH_lowerleg() const {
+ return this->tensor_RH_lowerleg.getMass();
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::Vec3d& InertiaProperties<TRAIT>::getCOM_trunk() const {
+ return this->com_trunk;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::Vec3d& InertiaProperties<TRAIT>::getCOM_LF_hipassembly() const {
+ return this->com_LF_hipassembly;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::Vec3d& InertiaProperties<TRAIT>::getCOM_LF_upperleg() const {
+ return this->com_LF_upperleg;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::Vec3d& InertiaProperties<TRAIT>::getCOM_LF_lowerleg() const {
+ return this->com_LF_lowerleg;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::Vec3d& InertiaProperties<TRAIT>::getCOM_RF_hipassembly() const {
+ return this->com_RF_hipassembly;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::Vec3d& InertiaProperties<TRAIT>::getCOM_RF_upperleg() const {
+ return this->com_RF_upperleg;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::Vec3d& InertiaProperties<TRAIT>::getCOM_RF_lowerleg() const {
+ return this->com_RF_lowerleg;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::Vec3d& InertiaProperties<TRAIT>::getCOM_LH_hipassembly() const {
+ return this->com_LH_hipassembly;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::Vec3d& InertiaProperties<TRAIT>::getCOM_LH_upperleg() const {
+ return this->com_LH_upperleg;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::Vec3d& InertiaProperties<TRAIT>::getCOM_LH_lowerleg() const {
+ return this->com_LH_lowerleg;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::Vec3d& InertiaProperties<TRAIT>::getCOM_RH_hipassembly() const {
+ return this->com_RH_hipassembly;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::Vec3d& InertiaProperties<TRAIT>::getCOM_RH_upperleg() const {
+ return this->com_RH_upperleg;
+}
+template <typename TRAIT>
+inline const typename InertiaProperties<TRAIT>::Vec3d& InertiaProperties<TRAIT>::getCOM_RH_lowerleg() const {
+ return this->com_RH_lowerleg;
+}
+
+template <typename TRAIT>
+inline typename InertiaProperties<TRAIT>::Scalar InertiaProperties<TRAIT>::getTotalMass() const {
+ return 47.376 + 2.93 + 2.638 + 0.881 + 2.93 + 2.638 + 0.881 + 2.93 + 2.638 + 0.881 + 2.93 + 2.638 + 0.881;
+}
+
+}
+
+using InertiaProperties = tpl::InertiaProperties<rbd::DoubleTrait>;
+
+}
+}
+}
+
+#include "inertia_properties.impl.h"
+
+#endif
diff --git a/ct_rbd/test/models/testhyq/generated/inertia_properties.impl.h b/ct_rbd/test/models/testhyq/generated/inertia_properties.impl.h
new file mode 100644
index 0000000..18a80d5
--- /dev/null
+++ b/ct_rbd/test/models/testhyq/generated/inertia_properties.impl.h
@@ -0,0 +1,161 @@
+template <typename TRAIT>
+iit::TestHyQ::dyn::tpl::InertiaProperties<TRAIT>::InertiaProperties()
+{
+ com_trunk = iit::rbd::Vector3d(-0.0223,-0.0,0.0387).cast<Scalar>();
+ tensor_trunk.fill(
+ Scalar(47.376),
+ com_trunk,
+ rbd::Utils::buildInertiaTensor(
+ Scalar(1.209488),
+ Scalar(5.5837),
+ Scalar(6.056973),
+ Scalar(0.00571),
+ Scalar(-0.190812),
+ Scalar(-0.012668)) );
+
+ com_LF_hipassembly = iit::rbd::Vector3d(0.043,0.0,0.169).cast<Scalar>();
+ tensor_LF_hipassembly.fill(
+ Scalar(2.93),
+ com_LF_hipassembly,
+ rbd::Utils::buildInertiaTensor(
+ Scalar(0.134705),
+ Scalar(0.144171),
+ Scalar(0.011033),
+ Scalar(3.6E-5),
+ Scalar(0.022734),
+ Scalar(5.1E-5)) );
+
+ com_LF_upperleg = iit::rbd::Vector3d(0.151,-0.026,-0.0).cast<Scalar>();
+ tensor_LF_upperleg.fill(
+ Scalar(2.638),
+ com_LF_upperleg,
+ rbd::Utils::buildInertiaTensor(
+ Scalar(0.005495),
+ Scalar(0.087136),
+ Scalar(0.089871),
+ Scalar(-0.007418),
+ Scalar(-1.02E-4),
+ Scalar(-2.1E-5)) );
+
+ com_LF_lowerleg = iit::rbd::Vector3d(0.125,0.0,-0.0).cast<Scalar>();
+ tensor_LF_lowerleg.fill(
+ Scalar(0.881),
+ com_LF_lowerleg,
+ rbd::Utils::buildInertiaTensor(
+ Scalar(4.68E-4),
+ Scalar(0.026409),
+ Scalar(0.026181),
+ Scalar(0.0),
+ Scalar(0.0),
+ Scalar(0.0)) );
+
+ com_RF_hipassembly = iit::rbd::Vector3d(0.043,-0.0,-0.169).cast<Scalar>();
+ tensor_RF_hipassembly.fill(
+ Scalar(2.93),
+ com_RF_hipassembly,
+ rbd::Utils::buildInertiaTensor(
+ Scalar(0.134705),
+ Scalar(0.144171),
+ Scalar(0.011033),
+ Scalar(-3.6E-5),
+ Scalar(-0.022734),
+ Scalar(5.1E-5)) );
+
+ com_RF_upperleg = iit::rbd::Vector3d(0.151,-0.026,-0.0).cast<Scalar>();
+ tensor_RF_upperleg.fill(
+ Scalar(2.638),
+ com_RF_upperleg,
+ rbd::Utils::buildInertiaTensor(
+ Scalar(0.005495),
+ Scalar(0.087136),
+ Scalar(0.089871),
+ Scalar(-0.007418),
+ Scalar(-1.02E-4),
+ Scalar(-2.1E-5)) );
+
+ com_RF_lowerleg = iit::rbd::Vector3d(0.125,0.001,-0.0).cast<Scalar>();
+ tensor_RF_lowerleg.fill(
+ Scalar(0.881),
+ com_RF_lowerleg,
+ rbd::Utils::buildInertiaTensor(
+ Scalar(4.68E-4),
+ Scalar(0.026409),
+ Scalar(0.026181),
+ Scalar(0.0),
+ Scalar(0.0),
+ Scalar(0.0)) );
+
+ com_LH_hipassembly = iit::rbd::Vector3d(0.043,-0.0,-0.169).cast<Scalar>();
+ tensor_LH_hipassembly.fill(
+ Scalar(2.93),
+ com_LH_hipassembly,
+ rbd::Utils::buildInertiaTensor(
+ Scalar(0.134705),
+ Scalar(0.144171),
+ Scalar(0.011033),
+ Scalar(-3.6E-5),
+ Scalar(-0.022734),
+ Scalar(5.1E-5)) );
+
+ com_LH_upperleg = iit::rbd::Vector3d(0.151,0.026,0.0).cast<Scalar>();
+ tensor_LH_upperleg.fill(
+ Scalar(2.638),
+ com_LH_upperleg,
+ rbd::Utils::buildInertiaTensor(
+ Scalar(0.005495),
+ Scalar(0.087136),
+ Scalar(0.089871),
+ Scalar(0.007418),
+ Scalar(1.02E-4),
+ Scalar(-2.1E-5)) );
+
+ com_LH_lowerleg = iit::rbd::Vector3d(0.125,-0.001,0.0).cast<Scalar>();
+ tensor_LH_lowerleg.fill(
+ Scalar(0.881),
+ com_LH_lowerleg,
+ rbd::Utils::buildInertiaTensor(
+ Scalar(4.68E-4),
+ Scalar(0.026409),
+ Scalar(0.026181),
+ Scalar(0.0),
+ Scalar(0.0),
+ Scalar(0.0)) );
+
+ com_RH_hipassembly = iit::rbd::Vector3d(0.043,0.0,0.169).cast<Scalar>();
+ tensor_RH_hipassembly.fill(
+ Scalar(2.93),
+ com_RH_hipassembly,
+ rbd::Utils::buildInertiaTensor(
+ Scalar(0.134705),
+ Scalar(0.144171),
+ Scalar(0.011033),
+ Scalar(3.6E-5),
+ Scalar(0.022734),
+ Scalar(5.1E-5)) );
+
+ com_RH_upperleg = iit::rbd::Vector3d(0.151,0.026,0.0).cast<Scalar>();
+ tensor_RH_upperleg.fill(
+ Scalar(2.638),
+ com_RH_upperleg,
+ rbd::Utils::buildInertiaTensor(
+ Scalar(0.005495),
+ Scalar(0.087136),
+ Scalar(0.089871),
+ Scalar(0.007418),
+ Scalar(1.02E-4),
+ Scalar(-2.1E-5)) );
+
+ com_RH_lowerleg = iit::rbd::Vector3d(0.125,-0.001,0.0).cast<Scalar>();
+ tensor_RH_lowerleg.fill(
+ Scalar(0.881),
+ com_RH_lowerleg,
+ rbd::Utils::buildInertiaTensor(
+ Scalar(4.68E-4),
+ Scalar(0.026409),
+ Scalar(0.026181),
+ Scalar(0.0),
+ Scalar(0.0),
+ Scalar(0.0)) );
+
+}
+
diff --git a/ct_rbd/test/models/testhyq/generated/inverse_dynamics.h b/ct_rbd/test/models/testhyq/generated/inverse_dynamics.h
new file mode 100644
index 0000000..2c11c87
--- /dev/null
+++ b/ct_rbd/test/models/testhyq/generated/inverse_dynamics.h
@@ -0,0 +1,373 @@
+#ifndef IIT_TESTHYQ_INVERSE_DYNAMICS_H_
+#define IIT_TESTHYQ_INVERSE_DYNAMICS_H_
+
+#include <iit/rbd/rbd.h>
+#include <iit/rbd/InertiaMatrix.h>
+#include <iit/rbd/utils.h>
+#include <iit/rbd/robcogen_commons.h>
+#include <iit/rbd/traits/DoubleTrait.h>
+
+#include "declarations.h"
+#include "inertia_properties.h"
+#include "transforms.h"
+#include "link_data_map.h"
+
+namespace iit {
+namespace TestHyQ {
+namespace dyn {
+
+/**
+ * The Inverse Dynamics routine for the robot TestHyQ.
+ *
+ * In addition to the full Newton-Euler algorithm, specialized versions to compute
+ * only certain terms are provided.
+ * The parameters common to most of the methods are the joint status vector \c q, the
+ * joint velocity vector \c qd and the acceleration vector \c qdd.
+ *
+ * Additional overloaded methods are provided without the \c q parameter. These
+ * methods use the current configuration of the robot; they are provided for the
+ * sake of efficiency, in case the motion transforms of the robot have already
+ * been updated elsewhere with the most recent configuration (eg by a call to
+ * setJointStatus()), so that it is useless to compute them again.
+ *
+ * Whenever present, the external forces parameter is a set of external
+ * wrenches acting on the robot links. Each wrench must be expressed in
+ * the reference frame of the link it is excerted on.
+ */
+
+namespace tpl {
+
+template <typename TRAIT>
+class InverseDynamics {
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef typename TRAIT::Scalar Scalar;
+
+ typedef iit::rbd::Core<Scalar> CoreS;
+
+ typedef typename CoreS::ForceVector Force;
+ typedef LinkDataMap<Force> ExtForces;
+ typedef typename CoreS::VelocityVector Velocity;
+ typedef typename CoreS::VelocityVector Acceleration;
+ typedef iit::rbd::tpl::InertiaMatrixDense<Scalar> InertiaMatrix;
+ typedef iit::TestHyQ::tpl::JointState<Scalar> JointState;
+ typedef typename CoreS::Matrix66 Matrix66s;
+ typedef iit::TestHyQ::tpl::MotionTransforms<TRAIT> MTransforms;
+ typedef InertiaProperties<TRAIT> IProperties;
+
+public:
+ /**
+ * Default constructor
+ * \param in the inertia properties of the links
+ * \param tr the container of all the spatial motion transforms of
+ * the robot TestHyQ, which will be used by this instance
+ * to compute inverse-dynamics.
+ */
+ InverseDynamics(IProperties& in, MTransforms& tr);
+
+ /** \name Inverse dynamics
+ * The full algorithm for the inverse dynamics of this robot.
+ *
+ * All the spatial vectors in the parameters are expressed in base coordinates,
+ * besides the external forces: each force must be expressed in the reference
+ * frame of the link it is acting on.
+ * \param[out] jForces the joint force vector required to achieve the desired accelerations
+ * \param[out] baseAccel the spatial acceleration of the robot base
+ * \param[in] g the gravity acceleration, as a spatial vector;
+ * gravity implicitly specifies the orientation of the base in space
+ * \param[in] trunk_v the spatial velocity of the base
+ * \param[in] q the joint position vector
+ * \param[in] qd the joint velocity vector
+ * \param[in] qdd the desired joint acceleration vector
+ * \param[in] fext the external forces acting on the links; this parameters
+ * defaults to zero
+ */ ///@{
+ void id(
+ JointState& jForces, Acceleration& trunk_a,
+ const Acceleration& g, const Velocity& trunk_v,
+ const JointState& q, const JointState& qd, const JointState& qdd,
+ const ExtForces& fext = zeroExtForces);
+ void id(
+ JointState& jForces, Acceleration& trunk_a,
+ const Acceleration& g, const Velocity& trunk_v,
+ const JointState& qd, const JointState& qdd,
+ const ExtForces& fext = zeroExtForces);
+ ///@}
+ /** \name Inverse dynamics, fully actuated base
+ * The inverse dynamics algorithm for the floating base robot,
+ * in the assumption of a fully actuated base.
+ *
+ * All the spatial vectors in the parameters are expressed in base coordinates,
+ * besides the external forces: each force must be expressed in the reference
+ * frame of the link it is acting on.
+ * \param[out] baseWrench the spatial force to be applied to the robot base to achieve
+ * the desired accelerations
+ * \param[out] jForces the joint force vector required to achieve the desired accelerations
+ * \param[in] g the gravity acceleration, as a spatial vector;
+ * gravity implicitly specifies the orientation of the base in space
+ * \param[in] trunk_v the spatial velocity of the base
+ * \param[in] baseAccel the desired spatial acceleration of the robot base
+ * \param[in] q the joint position vector
+ * \param[in] qd the joint velocity vector
+ * \param[in] qdd the desired joint acceleration vector
+ * \param[in] fext the external forces acting on the links; this parameters
+ * defaults to zero
+ */ ///@{
+ void id_fully_actuated(
+ Force& baseWrench, JointState& jForces,
+ const Acceleration& g, const Velocity& trunk_v, const Acceleration& baseAccel,
+ const JointState& q, const JointState& qd, const JointState& qdd, const ExtForces& fext = zeroExtForces);
+ void id_fully_actuated(
+ Force& baseWrench, JointState& jForces,
+ const Acceleration& g, const Velocity& trunk_v, const Acceleration& baseAccel,
+ const JointState& qd, const JointState& qdd, const ExtForces& fext = zeroExtForces);
+ ///@}
+ /** \name Gravity terms, fully actuated base
+ */
+ ///@{
+ void G_terms_fully_actuated(
+ Force& baseWrench, JointState& jForces,
+ const Acceleration& g, const JointState& q);
+ void G_terms_fully_actuated(
+ Force& baseWrench, JointState& jForces,
+ const Acceleration& g);
+ ///@}
+ /** \name Centrifugal and Coriolis terms, fully actuated base
+ *
+ * These functions take only velocity inputs, that is, they assume
+ * a zero spatial acceleration of the base (in addition to zero acceleration
+ * at the actuated joints).
+ * Note that this is NOT the same as imposing zero acceleration
+ * at the virtual 6-dof-floting-base joint, which would result, in general,
+ * in a non-zero spatial acceleration of the base, due to velocity
+ * product terms.
+ */
+ ///@{
+ void C_terms_fully_actuated(
+ Force& baseWrench, JointState& jForces,
+ const Velocity& trunk_v, const JointState& q, const JointState& qd);
+ void C_terms_fully_actuated(
+ Force& baseWrench, JointState& jForces,
+ const Velocity& trunk_v, const JointState& qd);
+ ///@}
+ /** Updates all the kinematics transforms used by the inverse dynamics routine. */
+ void setJointStatus(const JointState& q) const;
+
+public:
+ /** \name Getters
+ * These functions return various spatial quantities used internally
+ * by the inverse dynamics routines, like the spatial acceleration
+ * of the links.
+ *
+ * The getters can be useful to retrieve the additional data that is not
+ * returned explicitly by the inverse dynamics routines even though it
+ * is computed. For example, after a call to the inverse dynamics,
+ * the spatial velocity of all the links has been determined and
+ * can be accessed.
+ *
+ * However, beware that certain routines might not use some of the
+ * spatial quantities, which therefore would retain their last value
+ * without being updated nor reset (for example, the spatial velocity
+ * of the links is unaffected by the computation of the gravity terms).
+ */
+ ///@{
+ const Force& getForce_trunk() const { return trunk_f; }
+ const Velocity& getVelocity_LF_hipassembly() const { return LF_hipassembly_v; }
+ const Acceleration& getAcceleration_LF_hipassembly() const { return LF_hipassembly_a; }
+ const Force& getForce_LF_hipassembly() const { return LF_hipassembly_f; }
+ const Velocity& getVelocity_LF_upperleg() const { return LF_upperleg_v; }
+ const Acceleration& getAcceleration_LF_upperleg() const { return LF_upperleg_a; }
+ const Force& getForce_LF_upperleg() const { return LF_upperleg_f; }
+ const Velocity& getVelocity_LF_lowerleg() const { return LF_lowerleg_v; }
+ const Acceleration& getAcceleration_LF_lowerleg() const { return LF_lowerleg_a; }
+ const Force& getForce_LF_lowerleg() const { return LF_lowerleg_f; }
+ const Velocity& getVelocity_RF_hipassembly() const { return RF_hipassembly_v; }
+ const Acceleration& getAcceleration_RF_hipassembly() const { return RF_hipassembly_a; }
+ const Force& getForce_RF_hipassembly() const { return RF_hipassembly_f; }
+ const Velocity& getVelocity_RF_upperleg() const { return RF_upperleg_v; }
+ const Acceleration& getAcceleration_RF_upperleg() const { return RF_upperleg_a; }
+ const Force& getForce_RF_upperleg() const { return RF_upperleg_f; }
+ const Velocity& getVelocity_RF_lowerleg() const { return RF_lowerleg_v; }
+ const Acceleration& getAcceleration_RF_lowerleg() const { return RF_lowerleg_a; }
+ const Force& getForce_RF_lowerleg() const { return RF_lowerleg_f; }
+ const Velocity& getVelocity_LH_hipassembly() const { return LH_hipassembly_v; }
+ const Acceleration& getAcceleration_LH_hipassembly() const { return LH_hipassembly_a; }
+ const Force& getForce_LH_hipassembly() const { return LH_hipassembly_f; }
+ const Velocity& getVelocity_LH_upperleg() const { return LH_upperleg_v; }
+ const Acceleration& getAcceleration_LH_upperleg() const { return LH_upperleg_a; }
+ const Force& getForce_LH_upperleg() const { return LH_upperleg_f; }
+ const Velocity& getVelocity_LH_lowerleg() const { return LH_lowerleg_v; }
+ const Acceleration& getAcceleration_LH_lowerleg() const { return LH_lowerleg_a; }
+ const Force& getForce_LH_lowerleg() const { return LH_lowerleg_f; }
+ const Velocity& getVelocity_RH_hipassembly() const { return RH_hipassembly_v; }
+ const Acceleration& getAcceleration_RH_hipassembly() const { return RH_hipassembly_a; }
+ const Force& getForce_RH_hipassembly() const { return RH_hipassembly_f; }
+ const Velocity& getVelocity_RH_upperleg() const { return RH_upperleg_v; }
+ const Acceleration& getAcceleration_RH_upperleg() const { return RH_upperleg_a; }
+ const Force& getForce_RH_upperleg() const { return RH_upperleg_f; }
+ const Velocity& getVelocity_RH_lowerleg() const { return RH_lowerleg_v; }
+ const Acceleration& getAcceleration_RH_lowerleg() const { return RH_lowerleg_a; }
+ const Force& getForce_RH_lowerleg() const { return RH_lowerleg_f; }
+ ///@}
+protected:
+ void secondPass_fullyActuated(JointState& jForces);
+
+private:
+ IProperties* inertiaProps;
+ MTransforms* xm;
+private:
+ Matrix66s vcross; // support variable
+ // Link 'LF_hipassembly' :
+ const InertiaMatrix& LF_hipassembly_I;
+ Velocity LF_hipassembly_v;
+ Acceleration LF_hipassembly_a;
+ Force LF_hipassembly_f;
+ // Link 'LF_upperleg' :
+ const InertiaMatrix& LF_upperleg_I;
+ Velocity LF_upperleg_v;
+ Acceleration LF_upperleg_a;
+ Force LF_upperleg_f;
+ // Link 'LF_lowerleg' :
+ const InertiaMatrix& LF_lowerleg_I;
+ Velocity LF_lowerleg_v;
+ Acceleration LF_lowerleg_a;
+ Force LF_lowerleg_f;
+ // Link 'RF_hipassembly' :
+ const InertiaMatrix& RF_hipassembly_I;
+ Velocity RF_hipassembly_v;
+ Acceleration RF_hipassembly_a;
+ Force RF_hipassembly_f;
+ // Link 'RF_upperleg' :
+ const InertiaMatrix& RF_upperleg_I;
+ Velocity RF_upperleg_v;
+ Acceleration RF_upperleg_a;
+ Force RF_upperleg_f;
+ // Link 'RF_lowerleg' :
+ const InertiaMatrix& RF_lowerleg_I;
+ Velocity RF_lowerleg_v;
+ Acceleration RF_lowerleg_a;
+ Force RF_lowerleg_f;
+ // Link 'LH_hipassembly' :
+ const InertiaMatrix& LH_hipassembly_I;
+ Velocity LH_hipassembly_v;
+ Acceleration LH_hipassembly_a;
+ Force LH_hipassembly_f;
+ // Link 'LH_upperleg' :
+ const InertiaMatrix& LH_upperleg_I;
+ Velocity LH_upperleg_v;
+ Acceleration LH_upperleg_a;
+ Force LH_upperleg_f;
+ // Link 'LH_lowerleg' :
+ const InertiaMatrix& LH_lowerleg_I;
+ Velocity LH_lowerleg_v;
+ Acceleration LH_lowerleg_a;
+ Force LH_lowerleg_f;
+ // Link 'RH_hipassembly' :
+ const InertiaMatrix& RH_hipassembly_I;
+ Velocity RH_hipassembly_v;
+ Acceleration RH_hipassembly_a;
+ Force RH_hipassembly_f;
+ // Link 'RH_upperleg' :
+ const InertiaMatrix& RH_upperleg_I;
+ Velocity RH_upperleg_v;
+ Acceleration RH_upperleg_a;
+ Force RH_upperleg_f;
+ // Link 'RH_lowerleg' :
+ const InertiaMatrix& RH_lowerleg_I;
+ Velocity RH_lowerleg_v;
+ Acceleration RH_lowerleg_a;
+ Force RH_lowerleg_f;
+
+ // The robot base
+ const InertiaMatrix& trunk_I;
+ InertiaMatrix trunk_Ic;
+ Force trunk_f;
+ // The composite inertia tensors
+ InertiaMatrix LF_hipassembly_Ic;
+ InertiaMatrix LF_upperleg_Ic;
+ const InertiaMatrix& LF_lowerleg_Ic;
+ InertiaMatrix RF_hipassembly_Ic;
+ InertiaMatrix RF_upperleg_Ic;
+ const InertiaMatrix& RF_lowerleg_Ic;
+ InertiaMatrix LH_hipassembly_Ic;
+ InertiaMatrix LH_upperleg_Ic;
+ const InertiaMatrix& LH_lowerleg_Ic;
+ InertiaMatrix RH_hipassembly_Ic;
+ InertiaMatrix RH_upperleg_Ic;
+ const InertiaMatrix& RH_lowerleg_Ic;
+
+private:
+ static const ExtForces zeroExtForces;
+};
+
+template <typename TRAIT>
+inline void InverseDynamics<TRAIT>::setJointStatus(const JointState& q) const
+{
+ (xm->fr_LF_hipassembly_X_fr_trunk)(q);
+ (xm->fr_LF_upperleg_X_fr_LF_hipassembly)(q);
+ (xm->fr_LF_lowerleg_X_fr_LF_upperleg)(q);
+ (xm->fr_RF_hipassembly_X_fr_trunk)(q);
+ (xm->fr_RF_upperleg_X_fr_RF_hipassembly)(q);
+ (xm->fr_RF_lowerleg_X_fr_RF_upperleg)(q);
+ (xm->fr_LH_hipassembly_X_fr_trunk)(q);
+ (xm->fr_LH_upperleg_X_fr_LH_hipassembly)(q);
+ (xm->fr_LH_lowerleg_X_fr_LH_upperleg)(q);
+ (xm->fr_RH_hipassembly_X_fr_trunk)(q);
+ (xm->fr_RH_upperleg_X_fr_RH_hipassembly)(q);
+ (xm->fr_RH_lowerleg_X_fr_RH_upperleg)(q);
+}
+
+template <typename TRAIT>
+inline void InverseDynamics<TRAIT>::id(
+ JointState& jForces, Acceleration& trunk_a,
+ const Acceleration& g, const Velocity& trunk_v,
+ const JointState& q, const JointState& qd, const JointState& qdd,
+ const ExtForces& fext)
+{
+ setJointStatus(q);
+ id(jForces, trunk_a, g, trunk_v,
+ qd, qdd, fext);
+}
+
+template <typename TRAIT>
+inline void InverseDynamics<TRAIT>::G_terms_fully_actuated(
+ Force& baseWrench, JointState& jForces,
+ const Acceleration& g, const JointState& q)
+{
+ setJointStatus(q);
+ G_terms_fully_actuated(baseWrench, jForces, g);
+}
+
+template <typename TRAIT>
+inline void InverseDynamics<TRAIT>::C_terms_fully_actuated(
+ Force& baseWrench, JointState& jForces,
+ const Velocity& trunk_v, const JointState& q, const JointState& qd)
+{
+ setJointStatus(q);
+ C_terms_fully_actuated(baseWrench, jForces, trunk_v, qd);
+}
+
+template <typename TRAIT>
+inline void InverseDynamics<TRAIT>::id_fully_actuated(
+ Force& baseWrench, JointState& jForces,
+ const Acceleration& g, const Velocity& trunk_v, const Acceleration& baseAccel,
+ const JointState& q, const JointState& qd, const JointState& qdd, const ExtForces& fext)
+{
+ setJointStatus(q);
+ id_fully_actuated(baseWrench, jForces, g, trunk_v,
+ baseAccel, qd, qdd, fext);
+}
+
+}
+
+typedef tpl::InverseDynamics<rbd::DoubleTrait> InverseDynamics;
+
+}
+}
+
+}
+
+#include "inverse_dynamics.impl.h"
+
+#endif
diff --git a/ct_rbd/test/models/testhyq/generated/inverse_dynamics.impl.h b/ct_rbd/test/models/testhyq/generated/inverse_dynamics.impl.h
new file mode 100644
index 0000000..b52a6e2
--- /dev/null
+++ b/ct_rbd/test/models/testhyq/generated/inverse_dynamics.impl.h
@@ -0,0 +1,634 @@
+// Initialization of static-const data
+template <typename TRAIT>
+const typename iit::TestHyQ::dyn::tpl::InverseDynamics<TRAIT>::ExtForces
+iit::TestHyQ::dyn::tpl::InverseDynamics<TRAIT>::zeroExtForces(Force::Zero());
+
+template <typename TRAIT>
+iit::TestHyQ::dyn::tpl::InverseDynamics<TRAIT>::InverseDynamics(IProperties& inertia, MTransforms& transforms) :
+ inertiaProps( & inertia ),
+ xm( & transforms ),
+ LF_hipassembly_I(inertiaProps->getTensor_LF_hipassembly() ),
+ LF_upperleg_I(inertiaProps->getTensor_LF_upperleg() ),
+ LF_lowerleg_I(inertiaProps->getTensor_LF_lowerleg() ),
+ RF_hipassembly_I(inertiaProps->getTensor_RF_hipassembly() ),
+ RF_upperleg_I(inertiaProps->getTensor_RF_upperleg() ),
+ RF_lowerleg_I(inertiaProps->getTensor_RF_lowerleg() ),
+ LH_hipassembly_I(inertiaProps->getTensor_LH_hipassembly() ),
+ LH_upperleg_I(inertiaProps->getTensor_LH_upperleg() ),
+ LH_lowerleg_I(inertiaProps->getTensor_LH_lowerleg() ),
+ RH_hipassembly_I(inertiaProps->getTensor_RH_hipassembly() ),
+ RH_upperleg_I(inertiaProps->getTensor_RH_upperleg() ),
+ RH_lowerleg_I(inertiaProps->getTensor_RH_lowerleg() )
+ ,
+ trunk_I( inertiaProps->getTensor_trunk() ),
+ LF_lowerleg_Ic(LF_lowerleg_I),
+ RF_lowerleg_Ic(RF_lowerleg_I),
+ LH_lowerleg_Ic(LH_lowerleg_I),
+ RH_lowerleg_Ic(RH_lowerleg_I)
+{
+#ifndef EIGEN_NO_DEBUG
+ std::cout << "Robot TestHyQ, InverseDynamics<TRAIT>::InverseDynamics()" << std::endl;
+ std::cout << "Compiled with Eigen debug active" << std::endl;
+#endif
+ LF_hipassembly_v.setZero();
+ LF_upperleg_v.setZero();
+ LF_lowerleg_v.setZero();
+ RF_hipassembly_v.setZero();
+ RF_upperleg_v.setZero();
+ RF_lowerleg_v.setZero();
+ LH_hipassembly_v.setZero();
+ LH_upperleg_v.setZero();
+ LH_lowerleg_v.setZero();
+ RH_hipassembly_v.setZero();
+ RH_upperleg_v.setZero();
+ RH_lowerleg_v.setZero();
+
+ vcross.setZero();
+}
+
+template <typename TRAIT>
+void iit::TestHyQ::dyn::tpl::InverseDynamics<TRAIT>::id(
+ JointState& jForces, Acceleration& trunk_a,
+ const Acceleration& g, const Velocity& trunk_v,
+ const JointState& qd, const JointState& qdd,
+ const ExtForces& fext)
+{
+ trunk_Ic = trunk_I;
+ LF_hipassembly_Ic = LF_hipassembly_I;
+ LF_upperleg_Ic = LF_upperleg_I;
+ RF_hipassembly_Ic = RF_hipassembly_I;
+ RF_upperleg_Ic = RF_upperleg_I;
+ LH_hipassembly_Ic = LH_hipassembly_I;
+ LH_upperleg_Ic = LH_upperleg_I;
+ RH_hipassembly_Ic = RH_hipassembly_I;
+ RH_upperleg_Ic = RH_upperleg_I;
+
+ // First pass, link 'LF_hipassembly'
+ LF_hipassembly_v = ((xm->fr_LF_hipassembly_X_fr_trunk) * trunk_v);
+ LF_hipassembly_v(iit::rbd::AZ) += qd(LF_HAA);
+
+ iit::rbd::motionCrossProductMx<Scalar>(LF_hipassembly_v, vcross);
+
+ LF_hipassembly_a = (vcross.col(iit::rbd::AZ) * qd(LF_HAA));
+ LF_hipassembly_a(iit::rbd::AZ) += qdd(LF_HAA);
+
+ LF_hipassembly_f = LF_hipassembly_I * LF_hipassembly_a + iit::rbd::vxIv(LF_hipassembly_v, LF_hipassembly_I);
+
+ // First pass, link 'LF_upperleg'
+ LF_upperleg_v = ((xm->fr_LF_upperleg_X_fr_LF_hipassembly) * LF_hipassembly_v);
+ LF_upperleg_v(iit::rbd::AZ) += qd(LF_HFE);
+
+ iit::rbd::motionCrossProductMx<Scalar>(LF_upperleg_v, vcross);
+
+ LF_upperleg_a = (xm->fr_LF_upperleg_X_fr_LF_hipassembly) * LF_hipassembly_a + vcross.col(iit::rbd::AZ) * qd(LF_HFE);
+ LF_upperleg_a(iit::rbd::AZ) += qdd(LF_HFE);
+
+ LF_upperleg_f = LF_upperleg_I * LF_upperleg_a + iit::rbd::vxIv(LF_upperleg_v, LF_upperleg_I);
+
+ // First pass, link 'LF_lowerleg'
+ LF_lowerleg_v = ((xm->fr_LF_lowerleg_X_fr_LF_upperleg) * LF_upperleg_v);
+ LF_lowerleg_v(iit::rbd::AZ) += qd(LF_KFE);
+
+ iit::rbd::motionCrossProductMx<Scalar>(LF_lowerleg_v, vcross);
+
+ LF_lowerleg_a = (xm->fr_LF_lowerleg_X_fr_LF_upperleg) * LF_upperleg_a + vcross.col(iit::rbd::AZ) * qd(LF_KFE);
+ LF_lowerleg_a(iit::rbd::AZ) += qdd(LF_KFE);
+
+ LF_lowerleg_f = LF_lowerleg_I * LF_lowerleg_a + iit::rbd::vxIv(LF_lowerleg_v, LF_lowerleg_I);
+
+ // First pass, link 'RF_hipassembly'
+ RF_hipassembly_v = ((xm->fr_RF_hipassembly_X_fr_trunk) * trunk_v);
+ RF_hipassembly_v(iit::rbd::AZ) += qd(RF_HAA);
+
+ iit::rbd::motionCrossProductMx<Scalar>(RF_hipassembly_v, vcross);
+
+ RF_hipassembly_a = (vcross.col(iit::rbd::AZ) * qd(RF_HAA));
+ RF_hipassembly_a(iit::rbd::AZ) += qdd(RF_HAA);
+
+ RF_hipassembly_f = RF_hipassembly_I * RF_hipassembly_a + iit::rbd::vxIv(RF_hipassembly_v, RF_hipassembly_I);
+
+ // First pass, link 'RF_upperleg'
+ RF_upperleg_v = ((xm->fr_RF_upperleg_X_fr_RF_hipassembly) * RF_hipassembly_v);
+ RF_upperleg_v(iit::rbd::AZ) += qd(RF_HFE);
+
+ iit::rbd::motionCrossProductMx<Scalar>(RF_upperleg_v, vcross);
+
+ RF_upperleg_a = (xm->fr_RF_upperleg_X_fr_RF_hipassembly) * RF_hipassembly_a + vcross.col(iit::rbd::AZ) * qd(RF_HFE);
+ RF_upperleg_a(iit::rbd::AZ) += qdd(RF_HFE);
+
+ RF_upperleg_f = RF_upperleg_I * RF_upperleg_a + iit::rbd::vxIv(RF_upperleg_v, RF_upperleg_I);
+
+ // First pass, link 'RF_lowerleg'
+ RF_lowerleg_v = ((xm->fr_RF_lowerleg_X_fr_RF_upperleg) * RF_upperleg_v);
+ RF_lowerleg_v(iit::rbd::AZ) += qd(RF_KFE);
+
+ iit::rbd::motionCrossProductMx<Scalar>(RF_lowerleg_v, vcross);
+
+ RF_lowerleg_a = (xm->fr_RF_lowerleg_X_fr_RF_upperleg) * RF_upperleg_a + vcross.col(iit::rbd::AZ) * qd(RF_KFE);
+ RF_lowerleg_a(iit::rbd::AZ) += qdd(RF_KFE);
+
+ RF_lowerleg_f = RF_lowerleg_I * RF_lowerleg_a + iit::rbd::vxIv(RF_lowerleg_v, RF_lowerleg_I);
+
+ // First pass, link 'LH_hipassembly'
+ LH_hipassembly_v = ((xm->fr_LH_hipassembly_X_fr_trunk) * trunk_v);
+ LH_hipassembly_v(iit::rbd::AZ) += qd(LH_HAA);
+
+ iit::rbd::motionCrossProductMx<Scalar>(LH_hipassembly_v, vcross);
+
+ LH_hipassembly_a = (vcross.col(iit::rbd::AZ) * qd(LH_HAA));
+ LH_hipassembly_a(iit::rbd::AZ) += qdd(LH_HAA);
+
+ LH_hipassembly_f = LH_hipassembly_I * LH_hipassembly_a + iit::rbd::vxIv(LH_hipassembly_v, LH_hipassembly_I);
+
+ // First pass, link 'LH_upperleg'
+ LH_upperleg_v = ((xm->fr_LH_upperleg_X_fr_LH_hipassembly) * LH_hipassembly_v);
+ LH_upperleg_v(iit::rbd::AZ) += qd(LH_HFE);
+
+ iit::rbd::motionCrossProductMx<Scalar>(LH_upperleg_v, vcross);
+
+ LH_upperleg_a = (xm->fr_LH_upperleg_X_fr_LH_hipassembly) * LH_hipassembly_a + vcross.col(iit::rbd::AZ) * qd(LH_HFE);
+ LH_upperleg_a(iit::rbd::AZ) += qdd(LH_HFE);
+
+ LH_upperleg_f = LH_upperleg_I * LH_upperleg_a + iit::rbd::vxIv(LH_upperleg_v, LH_upperleg_I);
+
+ // First pass, link 'LH_lowerleg'
+ LH_lowerleg_v = ((xm->fr_LH_lowerleg_X_fr_LH_upperleg) * LH_upperleg_v);
+ LH_lowerleg_v(iit::rbd::AZ) += qd(LH_KFE);
+
+ iit::rbd::motionCrossProductMx<Scalar>(LH_lowerleg_v, vcross);
+
+ LH_lowerleg_a = (xm->fr_LH_lowerleg_X_fr_LH_upperleg) * LH_upperleg_a + vcross.col(iit::rbd::AZ) * qd(LH_KFE);
+ LH_lowerleg_a(iit::rbd::AZ) += qdd(LH_KFE);
+
+ LH_lowerleg_f = LH_lowerleg_I * LH_lowerleg_a + iit::rbd::vxIv(LH_lowerleg_v, LH_lowerleg_I);
+
+ // First pass, link 'RH_hipassembly'
+ RH_hipassembly_v = ((xm->fr_RH_hipassembly_X_fr_trunk) * trunk_v);
+ RH_hipassembly_v(iit::rbd::AZ) += qd(RH_HAA);
+
+ iit::rbd::motionCrossProductMx<Scalar>(RH_hipassembly_v, vcross);
+
+ RH_hipassembly_a = (vcross.col(iit::rbd::AZ) * qd(RH_HAA));
+ RH_hipassembly_a(iit::rbd::AZ) += qdd(RH_HAA);
+
+ RH_hipassembly_f = RH_hipassembly_I * RH_hipassembly_a + iit::rbd::vxIv(RH_hipassembly_v, RH_hipassembly_I);
+
+ // First pass, link 'RH_upperleg'
+ RH_upperleg_v = ((xm->fr_RH_upperleg_X_fr_RH_hipassembly) * RH_hipassembly_v);
+ RH_upperleg_v(iit::rbd::AZ) += qd(RH_HFE);
+
+ iit::rbd::motionCrossProductMx<Scalar>(RH_upperleg_v, vcross);
+
+ RH_upperleg_a = (xm->fr_RH_upperleg_X_fr_RH_hipassembly) * RH_hipassembly_a + vcross.col(iit::rbd::AZ) * qd(RH_HFE);
+ RH_upperleg_a(iit::rbd::AZ) += qdd(RH_HFE);
+
+ RH_upperleg_f = RH_upperleg_I * RH_upperleg_a + iit::rbd::vxIv(RH_upperleg_v, RH_upperleg_I);
+
+ // First pass, link 'RH_lowerleg'
+ RH_lowerleg_v = ((xm->fr_RH_lowerleg_X_fr_RH_upperleg) * RH_upperleg_v);
+ RH_lowerleg_v(iit::rbd::AZ) += qd(RH_KFE);
+
+ iit::rbd::motionCrossProductMx<Scalar>(RH_lowerleg_v, vcross);
+
+ RH_lowerleg_a = (xm->fr_RH_lowerleg_X_fr_RH_upperleg) * RH_upperleg_a + vcross.col(iit::rbd::AZ) * qd(RH_KFE);
+ RH_lowerleg_a(iit::rbd::AZ) += qdd(RH_KFE);
+
+ RH_lowerleg_f = RH_lowerleg_I * RH_lowerleg_a + iit::rbd::vxIv(RH_lowerleg_v, RH_lowerleg_I);
+
+ // The force exerted on the floating base by the links
+ trunk_f = iit::rbd::vxIv(trunk_v, trunk_I);
+
+
+ // Add the external forces:
+ trunk_f -= fext[TRUNK];
+ LF_hipassembly_f -= fext[LF_HIPASSEMBLY];
+ LF_upperleg_f -= fext[LF_UPPERLEG];
+ LF_lowerleg_f -= fext[LF_LOWERLEG];
+ RF_hipassembly_f -= fext[RF_HIPASSEMBLY];
+ RF_upperleg_f -= fext[RF_UPPERLEG];
+ RF_lowerleg_f -= fext[RF_LOWERLEG];
+ LH_hipassembly_f -= fext[LH_HIPASSEMBLY];
+ LH_upperleg_f -= fext[LH_UPPERLEG];
+ LH_lowerleg_f -= fext[LH_LOWERLEG];
+ RH_hipassembly_f -= fext[RH_HIPASSEMBLY];
+ RH_upperleg_f -= fext[RH_UPPERLEG];
+ RH_lowerleg_f -= fext[RH_LOWERLEG];
+
+ RH_upperleg_Ic = RH_upperleg_Ic + (xm->fr_RH_lowerleg_X_fr_RH_upperleg).transpose() * RH_lowerleg_Ic * (xm->fr_RH_lowerleg_X_fr_RH_upperleg);
+ RH_upperleg_f = RH_upperleg_f + (xm->fr_RH_lowerleg_X_fr_RH_upperleg).transpose() * RH_lowerleg_f;
+
+ RH_hipassembly_Ic = RH_hipassembly_Ic + (xm->fr_RH_upperleg_X_fr_RH_hipassembly).transpose() * RH_upperleg_Ic * (xm->fr_RH_upperleg_X_fr_RH_hipassembly);
+ RH_hipassembly_f = RH_hipassembly_f + (xm->fr_RH_upperleg_X_fr_RH_hipassembly).transpose() * RH_upperleg_f;
+
+ trunk_Ic = trunk_Ic + (xm->fr_RH_hipassembly_X_fr_trunk).transpose() * RH_hipassembly_Ic * (xm->fr_RH_hipassembly_X_fr_trunk);
+ trunk_f = trunk_f + (xm->fr_RH_hipassembly_X_fr_trunk).transpose() * RH_hipassembly_f;
+
+ LH_upperleg_Ic = LH_upperleg_Ic + (xm->fr_LH_lowerleg_X_fr_LH_upperleg).transpose() * LH_lowerleg_Ic * (xm->fr_LH_lowerleg_X_fr_LH_upperleg);
+ LH_upperleg_f = LH_upperleg_f + (xm->fr_LH_lowerleg_X_fr_LH_upperleg).transpose() * LH_lowerleg_f;
+
+ LH_hipassembly_Ic = LH_hipassembly_Ic + (xm->fr_LH_upperleg_X_fr_LH_hipassembly).transpose() * LH_upperleg_Ic * (xm->fr_LH_upperleg_X_fr_LH_hipassembly);
+ LH_hipassembly_f = LH_hipassembly_f + (xm->fr_LH_upperleg_X_fr_LH_hipassembly).transpose() * LH_upperleg_f;
+
+ trunk_Ic = trunk_Ic + (xm->fr_LH_hipassembly_X_fr_trunk).transpose() * LH_hipassembly_Ic * (xm->fr_LH_hipassembly_X_fr_trunk);
+ trunk_f = trunk_f + (xm->fr_LH_hipassembly_X_fr_trunk).transpose() * LH_hipassembly_f;
+
+ RF_upperleg_Ic = RF_upperleg_Ic + (xm->fr_RF_lowerleg_X_fr_RF_upperleg).transpose() * RF_lowerleg_Ic * (xm->fr_RF_lowerleg_X_fr_RF_upperleg);
+ RF_upperleg_f = RF_upperleg_f + (xm->fr_RF_lowerleg_X_fr_RF_upperleg).transpose() * RF_lowerleg_f;
+
+ RF_hipassembly_Ic = RF_hipassembly_Ic + (xm->fr_RF_upperleg_X_fr_RF_hipassembly).transpose() * RF_upperleg_Ic * (xm->fr_RF_upperleg_X_fr_RF_hipassembly);
+ RF_hipassembly_f = RF_hipassembly_f + (xm->fr_RF_upperleg_X_fr_RF_hipassembly).transpose() * RF_upperleg_f;
+
+ trunk_Ic = trunk_Ic + (xm->fr_RF_hipassembly_X_fr_trunk).transpose() * RF_hipassembly_Ic * (xm->fr_RF_hipassembly_X_fr_trunk);
+ trunk_f = trunk_f + (xm->fr_RF_hipassembly_X_fr_trunk).transpose() * RF_hipassembly_f;
+
+ LF_upperleg_Ic = LF_upperleg_Ic + (xm->fr_LF_lowerleg_X_fr_LF_upperleg).transpose() * LF_lowerleg_Ic * (xm->fr_LF_lowerleg_X_fr_LF_upperleg);
+ LF_upperleg_f = LF_upperleg_f + (xm->fr_LF_lowerleg_X_fr_LF_upperleg).transpose() * LF_lowerleg_f;
+
+ LF_hipassembly_Ic = LF_hipassembly_Ic + (xm->fr_LF_upperleg_X_fr_LF_hipassembly).transpose() * LF_upperleg_Ic * (xm->fr_LF_upperleg_X_fr_LF_hipassembly);
+ LF_hipassembly_f = LF_hipassembly_f + (xm->fr_LF_upperleg_X_fr_LF_hipassembly).transpose() * LF_upperleg_f;
+
+ trunk_Ic = trunk_Ic + (xm->fr_LF_hipassembly_X_fr_trunk).transpose() * LF_hipassembly_Ic * (xm->fr_LF_hipassembly_X_fr_trunk);
+ trunk_f = trunk_f + (xm->fr_LF_hipassembly_X_fr_trunk).transpose() * LF_hipassembly_f;
+
+
+ // The base acceleration due to the force due to the movement of the links
+ trunk_a = - trunk_Ic.inverse() * trunk_f;
+
+ LF_hipassembly_a = xm->fr_LF_hipassembly_X_fr_trunk * trunk_a;
+ jForces(LF_HAA) = (LF_hipassembly_Ic.row(iit::rbd::AZ) * LF_hipassembly_a)(0) + LF_hipassembly_f(iit::rbd::AZ);
+
+ LF_upperleg_a = xm->fr_LF_upperleg_X_fr_LF_hipassembly * LF_hipassembly_a;
+ jForces(LF_HFE) = (LF_upperleg_Ic.row(iit::rbd::AZ) * LF_upperleg_a)(0) + LF_upperleg_f(iit::rbd::AZ);
+
+ LF_lowerleg_a = xm->fr_LF_lowerleg_X_fr_LF_upperleg * LF_upperleg_a;
+ jForces(LF_KFE) = (LF_lowerleg_Ic.row(iit::rbd::AZ) * LF_lowerleg_a)(0) + LF_lowerleg_f(iit::rbd::AZ);
+
+ RF_hipassembly_a = xm->fr_RF_hipassembly_X_fr_trunk * trunk_a;
+ jForces(RF_HAA) = (RF_hipassembly_Ic.row(iit::rbd::AZ) * RF_hipassembly_a)(0) + RF_hipassembly_f(iit::rbd::AZ);
+
+ RF_upperleg_a = xm->fr_RF_upperleg_X_fr_RF_hipassembly * RF_hipassembly_a;
+ jForces(RF_HFE) = (RF_upperleg_Ic.row(iit::rbd::AZ) * RF_upperleg_a)(0) + RF_upperleg_f(iit::rbd::AZ);
+
+ RF_lowerleg_a = xm->fr_RF_lowerleg_X_fr_RF_upperleg * RF_upperleg_a;
+ jForces(RF_KFE) = (RF_lowerleg_Ic.row(iit::rbd::AZ) * RF_lowerleg_a)(0) + RF_lowerleg_f(iit::rbd::AZ);
+
+ LH_hipassembly_a = xm->fr_LH_hipassembly_X_fr_trunk * trunk_a;
+ jForces(LH_HAA) = (LH_hipassembly_Ic.row(iit::rbd::AZ) * LH_hipassembly_a)(0) + LH_hipassembly_f(iit::rbd::AZ);
+
+ LH_upperleg_a = xm->fr_LH_upperleg_X_fr_LH_hipassembly * LH_hipassembly_a;
+ jForces(LH_HFE) = (LH_upperleg_Ic.row(iit::rbd::AZ) * LH_upperleg_a)(0) + LH_upperleg_f(iit::rbd::AZ);
+
+ LH_lowerleg_a = xm->fr_LH_lowerleg_X_fr_LH_upperleg * LH_upperleg_a;
+ jForces(LH_KFE) = (LH_lowerleg_Ic.row(iit::rbd::AZ) * LH_lowerleg_a)(0) + LH_lowerleg_f(iit::rbd::AZ);
+
+ RH_hipassembly_a = xm->fr_RH_hipassembly_X_fr_trunk * trunk_a;
+ jForces(RH_HAA) = (RH_hipassembly_Ic.row(iit::rbd::AZ) * RH_hipassembly_a)(0) + RH_hipassembly_f(iit::rbd::AZ);
+
+ RH_upperleg_a = xm->fr_RH_upperleg_X_fr_RH_hipassembly * RH_hipassembly_a;
+ jForces(RH_HFE) = (RH_upperleg_Ic.row(iit::rbd::AZ) * RH_upperleg_a)(0) + RH_upperleg_f(iit::rbd::AZ);
+
+ RH_lowerleg_a = xm->fr_RH_lowerleg_X_fr_RH_upperleg * RH_upperleg_a;
+ jForces(RH_KFE) = (RH_lowerleg_Ic.row(iit::rbd::AZ) * RH_lowerleg_a)(0) + RH_lowerleg_f(iit::rbd::AZ);
+
+
+ trunk_a += g;
+}
+
+template <typename TRAIT>
+void iit::TestHyQ::dyn::tpl::InverseDynamics<TRAIT>::G_terms_fully_actuated(
+ Force& baseWrench, JointState& jForces,
+ const Acceleration& g)
+{
+ const Acceleration& trunk_a = -g;
+
+ // Link 'LF_hipassembly'
+ LF_hipassembly_a = (xm->fr_LF_hipassembly_X_fr_trunk) * trunk_a;
+ LF_hipassembly_f = LF_hipassembly_I * LF_hipassembly_a;
+ // Link 'LF_upperleg'
+ LF_upperleg_a = (xm->fr_LF_upperleg_X_fr_LF_hipassembly) * LF_hipassembly_a;
+ LF_upperleg_f = LF_upperleg_I * LF_upperleg_a;
+ // Link 'LF_lowerleg'
+ LF_lowerleg_a = (xm->fr_LF_lowerleg_X_fr_LF_upperleg) * LF_upperleg_a;
+ LF_lowerleg_f = LF_lowerleg_I * LF_lowerleg_a;
+ // Link 'RF_hipassembly'
+ RF_hipassembly_a = (xm->fr_RF_hipassembly_X_fr_trunk) * trunk_a;
+ RF_hipassembly_f = RF_hipassembly_I * RF_hipassembly_a;
+ // Link 'RF_upperleg'
+ RF_upperleg_a = (xm->fr_RF_upperleg_X_fr_RF_hipassembly) * RF_hipassembly_a;
+ RF_upperleg_f = RF_upperleg_I * RF_upperleg_a;
+ // Link 'RF_lowerleg'
+ RF_lowerleg_a = (xm->fr_RF_lowerleg_X_fr_RF_upperleg) * RF_upperleg_a;
+ RF_lowerleg_f = RF_lowerleg_I * RF_lowerleg_a;
+ // Link 'LH_hipassembly'
+ LH_hipassembly_a = (xm->fr_LH_hipassembly_X_fr_trunk) * trunk_a;
+ LH_hipassembly_f = LH_hipassembly_I * LH_hipassembly_a;
+ // Link 'LH_upperleg'
+ LH_upperleg_a = (xm->fr_LH_upperleg_X_fr_LH_hipassembly) * LH_hipassembly_a;
+ LH_upperleg_f = LH_upperleg_I * LH_upperleg_a;
+ // Link 'LH_lowerleg'
+ LH_lowerleg_a = (xm->fr_LH_lowerleg_X_fr_LH_upperleg) * LH_upperleg_a;
+ LH_lowerleg_f = LH_lowerleg_I * LH_lowerleg_a;
+ // Link 'RH_hipassembly'
+ RH_hipassembly_a = (xm->fr_RH_hipassembly_X_fr_trunk) * trunk_a;
+ RH_hipassembly_f = RH_hipassembly_I * RH_hipassembly_a;
+ // Link 'RH_upperleg'
+ RH_upperleg_a = (xm->fr_RH_upperleg_X_fr_RH_hipassembly) * RH_hipassembly_a;
+ RH_upperleg_f = RH_upperleg_I * RH_upperleg_a;
+ // Link 'RH_lowerleg'
+ RH_lowerleg_a = (xm->fr_RH_lowerleg_X_fr_RH_upperleg) * RH_upperleg_a;
+ RH_lowerleg_f = RH_lowerleg_I * RH_lowerleg_a;
+
+ trunk_f = trunk_I * trunk_a;
+
+ secondPass_fullyActuated(jForces);
+
+ baseWrench = trunk_f;
+}
+
+template <typename TRAIT>
+void iit::TestHyQ::dyn::tpl::InverseDynamics<TRAIT>::C_terms_fully_actuated(
+ Force& baseWrench, JointState& jForces,
+ const Velocity& trunk_v, const JointState& qd)
+{
+ // Link 'LF_hipassembly'
+ LF_hipassembly_v = ((xm->fr_LF_hipassembly_X_fr_trunk) * trunk_v);
+ LF_hipassembly_v(iit::rbd::AZ) += qd(LF_HAA);
+ iit::rbd::motionCrossProductMx<Scalar>(LF_hipassembly_v, vcross);
+ LF_hipassembly_a = (vcross.col(iit::rbd::AZ) * qd(LF_HAA));
+ LF_hipassembly_f = LF_hipassembly_I * LF_hipassembly_a + iit::rbd::vxIv(LF_hipassembly_v, LF_hipassembly_I);
+
+ // Link 'LF_upperleg'
+ LF_upperleg_v = ((xm->fr_LF_upperleg_X_fr_LF_hipassembly) * LF_hipassembly_v);
+ LF_upperleg_v(iit::rbd::AZ) += qd(LF_HFE);
+ iit::rbd::motionCrossProductMx<Scalar>(LF_upperleg_v, vcross);
+ LF_upperleg_a = (xm->fr_LF_upperleg_X_fr_LF_hipassembly) * LF_hipassembly_a + vcross.col(iit::rbd::AZ) * qd(LF_HFE);
+ LF_upperleg_f = LF_upperleg_I * LF_upperleg_a + iit::rbd::vxIv(LF_upperleg_v, LF_upperleg_I);
+
+ // Link 'LF_lowerleg'
+ LF_lowerleg_v = ((xm->fr_LF_lowerleg_X_fr_LF_upperleg) * LF_upperleg_v);
+ LF_lowerleg_v(iit::rbd::AZ) += qd(LF_KFE);
+ iit::rbd::motionCrossProductMx<Scalar>(LF_lowerleg_v, vcross);
+ LF_lowerleg_a = (xm->fr_LF_lowerleg_X_fr_LF_upperleg) * LF_upperleg_a + vcross.col(iit::rbd::AZ) * qd(LF_KFE);
+ LF_lowerleg_f = LF_lowerleg_I * LF_lowerleg_a + iit::rbd::vxIv(LF_lowerleg_v, LF_lowerleg_I);
+
+ // Link 'RF_hipassembly'
+ RF_hipassembly_v = ((xm->fr_RF_hipassembly_X_fr_trunk) * trunk_v);
+ RF_hipassembly_v(iit::rbd::AZ) += qd(RF_HAA);
+ iit::rbd::motionCrossProductMx<Scalar>(RF_hipassembly_v, vcross);
+ RF_hipassembly_a = (vcross.col(iit::rbd::AZ) * qd(RF_HAA));
+ RF_hipassembly_f = RF_hipassembly_I * RF_hipassembly_a + iit::rbd::vxIv(RF_hipassembly_v, RF_hipassembly_I);
+
+ // Link 'RF_upperleg'
+ RF_upperleg_v = ((xm->fr_RF_upperleg_X_fr_RF_hipassembly) * RF_hipassembly_v);
+ RF_upperleg_v(iit::rbd::AZ) += qd(RF_HFE);
+ iit::rbd::motionCrossProductMx<Scalar>(RF_upperleg_v, vcross);
+ RF_upperleg_a = (xm->fr_RF_upperleg_X_fr_RF_hipassembly) * RF_hipassembly_a + vcross.col(iit::rbd::AZ) * qd(RF_HFE);
+ RF_upperleg_f = RF_upperleg_I * RF_upperleg_a + iit::rbd::vxIv(RF_upperleg_v, RF_upperleg_I);
+
+ // Link 'RF_lowerleg'
+ RF_lowerleg_v = ((xm->fr_RF_lowerleg_X_fr_RF_upperleg) * RF_upperleg_v);
+ RF_lowerleg_v(iit::rbd::AZ) += qd(RF_KFE);
+ iit::rbd::motionCrossProductMx<Scalar>(RF_lowerleg_v, vcross);
+ RF_lowerleg_a = (xm->fr_RF_lowerleg_X_fr_RF_upperleg) * RF_upperleg_a + vcross.col(iit::rbd::AZ) * qd(RF_KFE);
+ RF_lowerleg_f = RF_lowerleg_I * RF_lowerleg_a + iit::rbd::vxIv(RF_lowerleg_v, RF_lowerleg_I);
+
+ // Link 'LH_hipassembly'
+ LH_hipassembly_v = ((xm->fr_LH_hipassembly_X_fr_trunk) * trunk_v);
+ LH_hipassembly_v(iit::rbd::AZ) += qd(LH_HAA);
+ iit::rbd::motionCrossProductMx<Scalar>(LH_hipassembly_v, vcross);
+ LH_hipassembly_a = (vcross.col(iit::rbd::AZ) * qd(LH_HAA));
+ LH_hipassembly_f = LH_hipassembly_I * LH_hipassembly_a + iit::rbd::vxIv(LH_hipassembly_v, LH_hipassembly_I);
+
+ // Link 'LH_upperleg'
+ LH_upperleg_v = ((xm->fr_LH_upperleg_X_fr_LH_hipassembly) * LH_hipassembly_v);
+ LH_upperleg_v(iit::rbd::AZ) += qd(LH_HFE);
+ iit::rbd::motionCrossProductMx<Scalar>(LH_upperleg_v, vcross);
+ LH_upperleg_a = (xm->fr_LH_upperleg_X_fr_LH_hipassembly) * LH_hipassembly_a + vcross.col(iit::rbd::AZ) * qd(LH_HFE);
+ LH_upperleg_f = LH_upperleg_I * LH_upperleg_a + iit::rbd::vxIv(LH_upperleg_v, LH_upperleg_I);
+
+ // Link 'LH_lowerleg'
+ LH_lowerleg_v = ((xm->fr_LH_lowerleg_X_fr_LH_upperleg) * LH_upperleg_v);
+ LH_lowerleg_v(iit::rbd::AZ) += qd(LH_KFE);
+ iit::rbd::motionCrossProductMx<Scalar>(LH_lowerleg_v, vcross);
+ LH_lowerleg_a = (xm->fr_LH_lowerleg_X_fr_LH_upperleg) * LH_upperleg_a + vcross.col(iit::rbd::AZ) * qd(LH_KFE);
+ LH_lowerleg_f = LH_lowerleg_I * LH_lowerleg_a + iit::rbd::vxIv(LH_lowerleg_v, LH_lowerleg_I);
+
+ // Link 'RH_hipassembly'
+ RH_hipassembly_v = ((xm->fr_RH_hipassembly_X_fr_trunk) * trunk_v);
+ RH_hipassembly_v(iit::rbd::AZ) += qd(RH_HAA);
+ iit::rbd::motionCrossProductMx<Scalar>(RH_hipassembly_v, vcross);
+ RH_hipassembly_a = (vcross.col(iit::rbd::AZ) * qd(RH_HAA));
+ RH_hipassembly_f = RH_hipassembly_I * RH_hipassembly_a + iit::rbd::vxIv(RH_hipassembly_v, RH_hipassembly_I);
+
+ // Link 'RH_upperleg'
+ RH_upperleg_v = ((xm->fr_RH_upperleg_X_fr_RH_hipassembly) * RH_hipassembly_v);
+ RH_upperleg_v(iit::rbd::AZ) += qd(RH_HFE);
+ iit::rbd::motionCrossProductMx<Scalar>(RH_upperleg_v, vcross);
+ RH_upperleg_a = (xm->fr_RH_upperleg_X_fr_RH_hipassembly) * RH_hipassembly_a + vcross.col(iit::rbd::AZ) * qd(RH_HFE);
+ RH_upperleg_f = RH_upperleg_I * RH_upperleg_a + iit::rbd::vxIv(RH_upperleg_v, RH_upperleg_I);
+
+ // Link 'RH_lowerleg'
+ RH_lowerleg_v = ((xm->fr_RH_lowerleg_X_fr_RH_upperleg) * RH_upperleg_v);
+ RH_lowerleg_v(iit::rbd::AZ) += qd(RH_KFE);
+ iit::rbd::motionCrossProductMx<Scalar>(RH_lowerleg_v, vcross);
+ RH_lowerleg_a = (xm->fr_RH_lowerleg_X_fr_RH_upperleg) * RH_upperleg_a + vcross.col(iit::rbd::AZ) * qd(RH_KFE);
+ RH_lowerleg_f = RH_lowerleg_I * RH_lowerleg_a + iit::rbd::vxIv(RH_lowerleg_v, RH_lowerleg_I);
+
+
+ trunk_f = iit::rbd::vxIv(trunk_v, trunk_I);
+
+ secondPass_fullyActuated(jForces);
+
+ baseWrench = trunk_f;
+}
+
+template <typename TRAIT>
+void iit::TestHyQ::dyn::tpl::InverseDynamics<TRAIT>::id_fully_actuated(
+ Force& baseWrench, JointState& jForces,
+ const Acceleration& g, const Velocity& trunk_v, const Acceleration& baseAccel,
+ const JointState& qd, const JointState& qdd, const ExtForces& fext)
+{
+ Acceleration trunk_a = baseAccel -g;
+
+ // First pass, link 'LF_hipassembly'
+ LF_hipassembly_v = ((xm->fr_LF_hipassembly_X_fr_trunk) * trunk_v);
+ LF_hipassembly_v(iit::rbd::AZ) += qd(LF_HAA);
+
+ iit::rbd::motionCrossProductMx<Scalar>(LF_hipassembly_v, vcross);
+
+ LF_hipassembly_a = (xm->fr_LF_hipassembly_X_fr_trunk) * trunk_a + vcross.col(iit::rbd::AZ) * qd(LF_HAA);
+ LF_hipassembly_a(iit::rbd::AZ) += qdd(LF_HAA);
+
+ LF_hipassembly_f = LF_hipassembly_I * LF_hipassembly_a + iit::rbd::vxIv(LF_hipassembly_v, LF_hipassembly_I) - fext[LF_HIPASSEMBLY];
+
+ // First pass, link 'LF_upperleg'
+ LF_upperleg_v = ((xm->fr_LF_upperleg_X_fr_LF_hipassembly) * LF_hipassembly_v);
+ LF_upperleg_v(iit::rbd::AZ) += qd(LF_HFE);
+
+ iit::rbd::motionCrossProductMx<Scalar>(LF_upperleg_v, vcross);
+
+ LF_upperleg_a = (xm->fr_LF_upperleg_X_fr_LF_hipassembly) * LF_hipassembly_a + vcross.col(iit::rbd::AZ) * qd(LF_HFE);
+ LF_upperleg_a(iit::rbd::AZ) += qdd(LF_HFE);
+
+ LF_upperleg_f = LF_upperleg_I * LF_upperleg_a + iit::rbd::vxIv(LF_upperleg_v, LF_upperleg_I) - fext[LF_UPPERLEG];
+
+ // First pass, link 'LF_lowerleg'
+ LF_lowerleg_v = ((xm->fr_LF_lowerleg_X_fr_LF_upperleg) * LF_upperleg_v);
+ LF_lowerleg_v(iit::rbd::AZ) += qd(LF_KFE);
+
+ iit::rbd::motionCrossProductMx<Scalar>(LF_lowerleg_v, vcross);
+
+ LF_lowerleg_a = (xm->fr_LF_lowerleg_X_fr_LF_upperleg) * LF_upperleg_a + vcross.col(iit::rbd::AZ) * qd(LF_KFE);
+ LF_lowerleg_a(iit::rbd::AZ) += qdd(LF_KFE);
+
+ LF_lowerleg_f = LF_lowerleg_I * LF_lowerleg_a + iit::rbd::vxIv(LF_lowerleg_v, LF_lowerleg_I) - fext[LF_LOWERLEG];
+
+ // First pass, link 'RF_hipassembly'
+ RF_hipassembly_v = ((xm->fr_RF_hipassembly_X_fr_trunk) * trunk_v);
+ RF_hipassembly_v(iit::rbd::AZ) += qd(RF_HAA);
+
+ iit::rbd::motionCrossProductMx<Scalar>(RF_hipassembly_v, vcross);
+
+ RF_hipassembly_a = (xm->fr_RF_hipassembly_X_fr_trunk) * trunk_a + vcross.col(iit::rbd::AZ) * qd(RF_HAA);
+ RF_hipassembly_a(iit::rbd::AZ) += qdd(RF_HAA);
+
+ RF_hipassembly_f = RF_hipassembly_I * RF_hipassembly_a + iit::rbd::vxIv(RF_hipassembly_v, RF_hipassembly_I) - fext[RF_HIPASSEMBLY];
+
+ // First pass, link 'RF_upperleg'
+ RF_upperleg_v = ((xm->fr_RF_upperleg_X_fr_RF_hipassembly) * RF_hipassembly_v);
+ RF_upperleg_v(iit::rbd::AZ) += qd(RF_HFE);
+
+ iit::rbd::motionCrossProductMx<Scalar>(RF_upperleg_v, vcross);
+
+ RF_upperleg_a = (xm->fr_RF_upperleg_X_fr_RF_hipassembly) * RF_hipassembly_a + vcross.col(iit::rbd::AZ) * qd(RF_HFE);
+ RF_upperleg_a(iit::rbd::AZ) += qdd(RF_HFE);
+
+ RF_upperleg_f = RF_upperleg_I * RF_upperleg_a + iit::rbd::vxIv(RF_upperleg_v, RF_upperleg_I) - fext[RF_UPPERLEG];
+
+ // First pass, link 'RF_lowerleg'
+ RF_lowerleg_v = ((xm->fr_RF_lowerleg_X_fr_RF_upperleg) * RF_upperleg_v);
+ RF_lowerleg_v(iit::rbd::AZ) += qd(RF_KFE);
+
+ iit::rbd::motionCrossProductMx<Scalar>(RF_lowerleg_v, vcross);
+
+ RF_lowerleg_a = (xm->fr_RF_lowerleg_X_fr_RF_upperleg) * RF_upperleg_a + vcross.col(iit::rbd::AZ) * qd(RF_KFE);
+ RF_lowerleg_a(iit::rbd::AZ) += qdd(RF_KFE);
+
+ RF_lowerleg_f = RF_lowerleg_I * RF_lowerleg_a + iit::rbd::vxIv(RF_lowerleg_v, RF_lowerleg_I) - fext[RF_LOWERLEG];
+
+ // First pass, link 'LH_hipassembly'
+ LH_hipassembly_v = ((xm->fr_LH_hipassembly_X_fr_trunk) * trunk_v);
+ LH_hipassembly_v(iit::rbd::AZ) += qd(LH_HAA);
+
+ iit::rbd::motionCrossProductMx<Scalar>(LH_hipassembly_v, vcross);
+
+ LH_hipassembly_a = (xm->fr_LH_hipassembly_X_fr_trunk) * trunk_a + vcross.col(iit::rbd::AZ) * qd(LH_HAA);
+ LH_hipassembly_a(iit::rbd::AZ) += qdd(LH_HAA);
+
+ LH_hipassembly_f = LH_hipassembly_I * LH_hipassembly_a + iit::rbd::vxIv(LH_hipassembly_v, LH_hipassembly_I) - fext[LH_HIPASSEMBLY];
+
+ // First pass, link 'LH_upperleg'
+ LH_upperleg_v = ((xm->fr_LH_upperleg_X_fr_LH_hipassembly) * LH_hipassembly_v);
+ LH_upperleg_v(iit::rbd::AZ) += qd(LH_HFE);
+
+ iit::rbd::motionCrossProductMx<Scalar>(LH_upperleg_v, vcross);
+
+ LH_upperleg_a = (xm->fr_LH_upperleg_X_fr_LH_hipassembly) * LH_hipassembly_a + vcross.col(iit::rbd::AZ) * qd(LH_HFE);
+ LH_upperleg_a(iit::rbd::AZ) += qdd(LH_HFE);
+
+ LH_upperleg_f = LH_upperleg_I * LH_upperleg_a + iit::rbd::vxIv(LH_upperleg_v, LH_upperleg_I) - fext[LH_UPPERLEG];
+
+ // First pass, link 'LH_lowerleg'
+ LH_lowerleg_v = ((xm->fr_LH_lowerleg_X_fr_LH_upperleg) * LH_upperleg_v);
+ LH_lowerleg_v(iit::rbd::AZ) += qd(LH_KFE);
+
+ iit::rbd::motionCrossProductMx<Scalar>(LH_lowerleg_v, vcross);
+
+ LH_lowerleg_a = (xm->fr_LH_lowerleg_X_fr_LH_upperleg) * LH_upperleg_a + vcross.col(iit::rbd::AZ) * qd(LH_KFE);
+ LH_lowerleg_a(iit::rbd::AZ) += qdd(LH_KFE);
+
+ LH_lowerleg_f = LH_lowerleg_I * LH_lowerleg_a + iit::rbd::vxIv(LH_lowerleg_v, LH_lowerleg_I) - fext[LH_LOWERLEG];
+
+ // First pass, link 'RH_hipassembly'
+ RH_hipassembly_v = ((xm->fr_RH_hipassembly_X_fr_trunk) * trunk_v);
+ RH_hipassembly_v(iit::rbd::AZ) += qd(RH_HAA);
+
+ iit::rbd::motionCrossProductMx<Scalar>(RH_hipassembly_v, vcross);
+
+ RH_hipassembly_a = (xm->fr_RH_hipassembly_X_fr_trunk) * trunk_a + vcross.col(iit::rbd::AZ) * qd(RH_HAA);
+ RH_hipassembly_a(iit::rbd::AZ) += qdd(RH_HAA);
+
+ RH_hipassembly_f = RH_hipassembly_I * RH_hipassembly_a + iit::rbd::vxIv(RH_hipassembly_v, RH_hipassembly_I) - fext[RH_HIPASSEMBLY];
+
+ // First pass, link 'RH_upperleg'
+ RH_upperleg_v = ((xm->fr_RH_upperleg_X_fr_RH_hipassembly) * RH_hipassembly_v);
+ RH_upperleg_v(iit::rbd::AZ) += qd(RH_HFE);
+
+ iit::rbd::motionCrossProductMx<Scalar>(RH_upperleg_v, vcross);
+
+ RH_upperleg_a = (xm->fr_RH_upperleg_X_fr_RH_hipassembly) * RH_hipassembly_a + vcross.col(iit::rbd::AZ) * qd(RH_HFE);
+ RH_upperleg_a(iit::rbd::AZ) += qdd(RH_HFE);
+
+ RH_upperleg_f = RH_upperleg_I * RH_upperleg_a + iit::rbd::vxIv(RH_upperleg_v, RH_upperleg_I) - fext[RH_UPPERLEG];
+
+ // First pass, link 'RH_lowerleg'
+ RH_lowerleg_v = ((xm->fr_RH_lowerleg_X_fr_RH_upperleg) * RH_upperleg_v);
+ RH_lowerleg_v(iit::rbd::AZ) += qd(RH_KFE);
+
+ iit::rbd::motionCrossProductMx<Scalar>(RH_lowerleg_v, vcross);
+
+ RH_lowerleg_a = (xm->fr_RH_lowerleg_X_fr_RH_upperleg) * RH_upperleg_a + vcross.col(iit::rbd::AZ) * qd(RH_KFE);
+ RH_lowerleg_a(iit::rbd::AZ) += qdd(RH_KFE);
+
+ RH_lowerleg_f = RH_lowerleg_I * RH_lowerleg_a + iit::rbd::vxIv(RH_lowerleg_v, RH_lowerleg_I) - fext[RH_LOWERLEG];
+
+
+ // The base
+ trunk_f = trunk_I * trunk_a + iit::rbd::vxIv(trunk_v, trunk_I) - fext[TRUNK];
+
+ secondPass_fullyActuated(jForces);
+
+ baseWrench = trunk_f;
+}
+
+template <typename TRAIT>
+void iit::TestHyQ::dyn::tpl::InverseDynamics<TRAIT>::secondPass_fullyActuated(JointState& jForces)
+{
+ // Link 'RH_lowerleg'
+ jForces(RH_KFE) = RH_lowerleg_f(iit::rbd::AZ);
+ RH_upperleg_f += xm->fr_RH_lowerleg_X_fr_RH_upperleg.transpose() * RH_lowerleg_f;
+ // Link 'RH_upperleg'
+ jForces(RH_HFE) = RH_upperleg_f(iit::rbd::AZ);
+ RH_hipassembly_f += xm->fr_RH_upperleg_X_fr_RH_hipassembly.transpose() * RH_upperleg_f;
+ // Link 'RH_hipassembly'
+ jForces(RH_HAA) = RH_hipassembly_f(iit::rbd::AZ);
+ trunk_f += xm->fr_RH_hipassembly_X_fr_trunk.transpose() * RH_hipassembly_f;
+ // Link 'LH_lowerleg'
+ jForces(LH_KFE) = LH_lowerleg_f(iit::rbd::AZ);
+ LH_upperleg_f += xm->fr_LH_lowerleg_X_fr_LH_upperleg.transpose() * LH_lowerleg_f;
+ // Link 'LH_upperleg'
+ jForces(LH_HFE) = LH_upperleg_f(iit::rbd::AZ);
+ LH_hipassembly_f += xm->fr_LH_upperleg_X_fr_LH_hipassembly.transpose() * LH_upperleg_f;
+ // Link 'LH_hipassembly'
+ jForces(LH_HAA) = LH_hipassembly_f(iit::rbd::AZ);
+ trunk_f += xm->fr_LH_hipassembly_X_fr_trunk.transpose() * LH_hipassembly_f;
+ // Link 'RF_lowerleg'
+ jForces(RF_KFE) = RF_lowerleg_f(iit::rbd::AZ);
+ RF_upperleg_f += xm->fr_RF_lowerleg_X_fr_RF_upperleg.transpose() * RF_lowerleg_f;
+ // Link 'RF_upperleg'
+ jForces(RF_HFE) = RF_upperleg_f(iit::rbd::AZ);
+ RF_hipassembly_f += xm->fr_RF_upperleg_X_fr_RF_hipassembly.transpose() * RF_upperleg_f;
+ // Link 'RF_hipassembly'
+ jForces(RF_HAA) = RF_hipassembly_f(iit::rbd::AZ);
+ trunk_f += xm->fr_RF_hipassembly_X_fr_trunk.transpose() * RF_hipassembly_f;
+ // Link 'LF_lowerleg'
+ jForces(LF_KFE) = LF_lowerleg_f(iit::rbd::AZ);
+ LF_upperleg_f += xm->fr_LF_lowerleg_X_fr_LF_upperleg.transpose() * LF_lowerleg_f;
+ // Link 'LF_upperleg'
+ jForces(LF_HFE) = LF_upperleg_f(iit::rbd::AZ);
+ LF_hipassembly_f += xm->fr_LF_upperleg_X_fr_LF_hipassembly.transpose() * LF_upperleg_f;
+ // Link 'LF_hipassembly'
+ jForces(LF_HAA) = LF_hipassembly_f(iit::rbd::AZ);
+ trunk_f += xm->fr_LF_hipassembly_X_fr_trunk.transpose() * LF_hipassembly_f;
+}
+
diff --git a/ct_rbd/test/models/testhyq/generated/jacobians.h b/ct_rbd/test/models/testhyq/generated/jacobians.h
new file mode 100644
index 0000000..84b9acf
--- /dev/null
+++ b/ct_rbd/test/models/testhyq/generated/jacobians.h
@@ -0,0 +1,211 @@
+#ifndef TESTHYQ_JACOBIANS_H_
+#define TESTHYQ_JACOBIANS_H_
+
+ #include <iit/rbd/rbd.h>
+#include <iit/rbd/TransformsBase.h>
+#include <iit/rbd/traits/DoubleTrait.h>
+#include "declarations.h"
+#include "kinematics_parameters.h"
+
+namespace iit {
+namespace TestHyQ {
+
+template<typename SCALAR, int COLS, class M>
+class JacobianT : public iit::rbd::JacobianBase<tpl::JointState<SCALAR>, COLS, M>
+{};
+
+namespace tpl {
+
+/**
+ *
+ */
+template <typename TRAIT>
+class Jacobians {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef typename TRAIT::Scalar Scalar;
+ typedef iit::rbd::Core<Scalar> CoreS;
+
+ typedef JointState<Scalar> JState;
+
+ class Type_fr_trunk_J_LF_hipassemblyCOM : public JacobianT<Scalar, 1, Type_fr_trunk_J_LF_hipassemblyCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_J_LF_hipassemblyCOM();
+ const Type_fr_trunk_J_LF_hipassemblyCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_J_RF_hipassemblyCOM : public JacobianT<Scalar, 1, Type_fr_trunk_J_RF_hipassemblyCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_J_RF_hipassemblyCOM();
+ const Type_fr_trunk_J_RF_hipassemblyCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_J_LH_hipassemblyCOM : public JacobianT<Scalar, 1, Type_fr_trunk_J_LH_hipassemblyCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_J_LH_hipassemblyCOM();
+ const Type_fr_trunk_J_LH_hipassemblyCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_J_RH_hipassemblyCOM : public JacobianT<Scalar, 1, Type_fr_trunk_J_RH_hipassemblyCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_J_RH_hipassemblyCOM();
+ const Type_fr_trunk_J_RH_hipassemblyCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_J_LF_upperlegCOM : public JacobianT<Scalar, 2, Type_fr_trunk_J_LF_upperlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_J_LF_upperlegCOM();
+ const Type_fr_trunk_J_LF_upperlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_J_RF_upperlegCOM : public JacobianT<Scalar, 2, Type_fr_trunk_J_RF_upperlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_J_RF_upperlegCOM();
+ const Type_fr_trunk_J_RF_upperlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_J_LH_upperlegCOM : public JacobianT<Scalar, 2, Type_fr_trunk_J_LH_upperlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_J_LH_upperlegCOM();
+ const Type_fr_trunk_J_LH_upperlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_J_RH_upperlegCOM : public JacobianT<Scalar, 2, Type_fr_trunk_J_RH_upperlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_J_RH_upperlegCOM();
+ const Type_fr_trunk_J_RH_upperlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_J_LF_lowerlegCOM : public JacobianT<Scalar, 3, Type_fr_trunk_J_LF_lowerlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_J_LF_lowerlegCOM();
+ const Type_fr_trunk_J_LF_lowerlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_J_RF_lowerlegCOM : public JacobianT<Scalar, 3, Type_fr_trunk_J_RF_lowerlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_J_RF_lowerlegCOM();
+ const Type_fr_trunk_J_RF_lowerlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_J_LH_lowerlegCOM : public JacobianT<Scalar, 3, Type_fr_trunk_J_LH_lowerlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_J_LH_lowerlegCOM();
+ const Type_fr_trunk_J_LH_lowerlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_J_RH_lowerlegCOM : public JacobianT<Scalar, 3, Type_fr_trunk_J_RH_lowerlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_J_RH_lowerlegCOM();
+ const Type_fr_trunk_J_RH_lowerlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_J_fr_LF_foot : public JacobianT<Scalar, 3, Type_fr_trunk_J_fr_LF_foot>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_J_fr_LF_foot();
+ const Type_fr_trunk_J_fr_LF_foot& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_J_fr_RF_foot : public JacobianT<Scalar, 3, Type_fr_trunk_J_fr_RF_foot>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_J_fr_RF_foot();
+ const Type_fr_trunk_J_fr_RF_foot& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_J_fr_LH_foot : public JacobianT<Scalar, 3, Type_fr_trunk_J_fr_LH_foot>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_J_fr_LH_foot();
+ const Type_fr_trunk_J_fr_LH_foot& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_J_fr_RH_foot : public JacobianT<Scalar, 3, Type_fr_trunk_J_fr_RH_foot>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_J_fr_RH_foot();
+ const Type_fr_trunk_J_fr_RH_foot& update(const JState&);
+ protected:
+ };
+
+ public:
+ Jacobians();
+ void updateParameters();
+ public:
+ Type_fr_trunk_J_LF_hipassemblyCOM fr_trunk_J_LF_hipassemblyCOM;
+ Type_fr_trunk_J_RF_hipassemblyCOM fr_trunk_J_RF_hipassemblyCOM;
+ Type_fr_trunk_J_LH_hipassemblyCOM fr_trunk_J_LH_hipassemblyCOM;
+ Type_fr_trunk_J_RH_hipassemblyCOM fr_trunk_J_RH_hipassemblyCOM;
+ Type_fr_trunk_J_LF_upperlegCOM fr_trunk_J_LF_upperlegCOM;
+ Type_fr_trunk_J_RF_upperlegCOM fr_trunk_J_RF_upperlegCOM;
+ Type_fr_trunk_J_LH_upperlegCOM fr_trunk_J_LH_upperlegCOM;
+ Type_fr_trunk_J_RH_upperlegCOM fr_trunk_J_RH_upperlegCOM;
+ Type_fr_trunk_J_LF_lowerlegCOM fr_trunk_J_LF_lowerlegCOM;
+ Type_fr_trunk_J_RF_lowerlegCOM fr_trunk_J_RF_lowerlegCOM;
+ Type_fr_trunk_J_LH_lowerlegCOM fr_trunk_J_LH_lowerlegCOM;
+ Type_fr_trunk_J_RH_lowerlegCOM fr_trunk_J_RH_lowerlegCOM;
+ Type_fr_trunk_J_fr_LF_foot fr_trunk_J_fr_LF_foot;
+ Type_fr_trunk_J_fr_RF_foot fr_trunk_J_fr_RF_foot;
+ Type_fr_trunk_J_fr_LH_foot fr_trunk_J_fr_LH_foot;
+ Type_fr_trunk_J_fr_RH_foot fr_trunk_J_fr_RH_foot;
+
+ protected:
+
+};
+
+} //namespace tpl
+
+using Jacobians = tpl::Jacobians<rbd::DoubleTrait>;
+
+#include "jacobians.impl.h"
+
+
+}
+}
+
+#endif
diff --git a/ct_rbd/test/models/testhyq/generated/jacobians.impl.h b/ct_rbd/test/models/testhyq/generated/jacobians.impl.h
new file mode 100644
index 0000000..ce218ee
--- /dev/null
+++ b/ct_rbd/test/models/testhyq/generated/jacobians.impl.h
@@ -0,0 +1,566 @@
+
+template <typename TRAIT>
+iit::TestHyQ::tpl::Jacobians<TRAIT>::Jacobians
+ ()
+ :
+ fr_trunk_J_LF_hipassemblyCOM(),
+ fr_trunk_J_RF_hipassemblyCOM(),
+ fr_trunk_J_LH_hipassemblyCOM(),
+ fr_trunk_J_RH_hipassemblyCOM(),
+ fr_trunk_J_LF_upperlegCOM(),
+ fr_trunk_J_RF_upperlegCOM(),
+ fr_trunk_J_LH_upperlegCOM(),
+ fr_trunk_J_RH_upperlegCOM(),
+ fr_trunk_J_LF_lowerlegCOM(),
+ fr_trunk_J_RF_lowerlegCOM(),
+ fr_trunk_J_LH_lowerlegCOM(),
+ fr_trunk_J_RH_lowerlegCOM(),
+ fr_trunk_J_fr_LF_foot(),
+ fr_trunk_J_fr_RF_foot(),
+ fr_trunk_J_fr_LH_foot(),
+ fr_trunk_J_fr_RH_foot()
+{
+ updateParameters();
+}
+
+template <typename TRAIT>
+void iit::TestHyQ::tpl::Jacobians<TRAIT>::updateParameters() {
+}
+
+
+template <typename TRAIT>
+iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_LF_hipassemblyCOM::Type_fr_trunk_J_LF_hipassemblyCOM()
+{
+ (*this)(0,0) = - 1.0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+}
+
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_LF_hipassemblyCOM& iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_LF_hipassemblyCOM::update(const JState& jState) {
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_HAA__ = TRAIT::sin( jState(LF_HAA));
+ cos__q_LF_HAA__ = TRAIT::cos( jState(LF_HAA));
+
+ (*this)(4,0) = (- 0.043 * cos__q_LF_HAA__);
+ (*this)(5,0) = ( 0.043 * sin__q_LF_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_RF_hipassemblyCOM::Type_fr_trunk_J_RF_hipassemblyCOM()
+{
+ (*this)(0,0) = 1.0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+}
+
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_RF_hipassemblyCOM& iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_RF_hipassemblyCOM::update(const JState& jState) {
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_HAA__ = TRAIT::sin( jState(RF_HAA));
+ cos__q_RF_HAA__ = TRAIT::cos( jState(RF_HAA));
+
+ (*this)(4,0) = ( 0.043 * cos__q_RF_HAA__);
+ (*this)(5,0) = ( 0.043 * sin__q_RF_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_LH_hipassemblyCOM::Type_fr_trunk_J_LH_hipassemblyCOM()
+{
+ (*this)(0,0) = - 1.0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+}
+
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_LH_hipassemblyCOM& iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_LH_hipassemblyCOM::update(const JState& jState) {
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_HAA__ = TRAIT::sin( jState(LH_HAA));
+ cos__q_LH_HAA__ = TRAIT::cos( jState(LH_HAA));
+
+ (*this)(4,0) = (- 0.043 * cos__q_LH_HAA__);
+ (*this)(5,0) = ( 0.043 * sin__q_LH_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_RH_hipassemblyCOM::Type_fr_trunk_J_RH_hipassemblyCOM()
+{
+ (*this)(0,0) = 1.0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+}
+
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_RH_hipassemblyCOM& iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_RH_hipassemblyCOM::update(const JState& jState) {
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_HAA__ = TRAIT::sin( jState(RH_HAA));
+ cos__q_RH_HAA__ = TRAIT::cos( jState(RH_HAA));
+
+ (*this)(4,0) = ( 0.043 * cos__q_RH_HAA__);
+ (*this)(5,0) = ( 0.043 * sin__q_RH_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_LF_upperlegCOM::Type_fr_trunk_J_LF_upperlegCOM()
+{
+ (*this)(0,0) = - 1.0;
+ (*this)(0,1) = 0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+}
+
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_LF_upperlegCOM& iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_LF_upperlegCOM::update(const JState& jState) {
+ Scalar sin__q_LF_HAA__;
+ Scalar sin__q_LF_HFE__;
+ Scalar cos__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+
+ sin__q_LF_HAA__ = TRAIT::sin( jState(LF_HAA));
+ sin__q_LF_HFE__ = TRAIT::sin( jState(LF_HFE));
+ cos__q_LF_HAA__ = TRAIT::cos( jState(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( jState(LF_HFE));
+
+ (*this)(1,1) = cos__q_LF_HAA__;
+ (*this)(2,1) = - sin__q_LF_HAA__;
+ (*this)(3,1) = ((- 0.026 * sin__q_LF_HFE__) - ( 0.151 * cos__q_LF_HFE__));
+ (*this)(4,0) = ((((- 0.026 * cos__q_LF_HAA__) * sin__q_LF_HFE__) - (( 0.151 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) - ( 0.08 * cos__q_LF_HAA__));
+ (*this)(4,1) = ((( 0.151 * sin__q_LF_HAA__) * sin__q_LF_HFE__) - (( 0.026 * sin__q_LF_HAA__) * cos__q_LF_HFE__));
+ (*this)(5,0) = (((( 0.026 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.151 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.08 * sin__q_LF_HAA__));
+ (*this)(5,1) = ((( 0.151 * cos__q_LF_HAA__) * sin__q_LF_HFE__) - (( 0.026 * cos__q_LF_HAA__) * cos__q_LF_HFE__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_RF_upperlegCOM::Type_fr_trunk_J_RF_upperlegCOM()
+{
+ (*this)(0,0) = 1.0;
+ (*this)(0,1) = 0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+}
+
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_RF_upperlegCOM& iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_RF_upperlegCOM::update(const JState& jState) {
+ Scalar sin__q_RF_HAA__;
+ Scalar sin__q_RF_HFE__;
+ Scalar cos__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+
+ sin__q_RF_HAA__ = TRAIT::sin( jState(RF_HAA));
+ sin__q_RF_HFE__ = TRAIT::sin( jState(RF_HFE));
+ cos__q_RF_HAA__ = TRAIT::cos( jState(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( jState(RF_HFE));
+
+ (*this)(1,1) = cos__q_RF_HAA__;
+ (*this)(2,1) = sin__q_RF_HAA__;
+ (*this)(3,1) = ((- 0.026 * sin__q_RF_HFE__) - ( 0.151 * cos__q_RF_HFE__));
+ (*this)(4,0) = (((( 0.026 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.151 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.08 * cos__q_RF_HAA__));
+ (*this)(4,1) = ((( 0.026 * sin__q_RF_HAA__) * cos__q_RF_HFE__) - (( 0.151 * sin__q_RF_HAA__) * sin__q_RF_HFE__));
+ (*this)(5,0) = (((( 0.026 * sin__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.151 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.08 * sin__q_RF_HAA__));
+ (*this)(5,1) = ((( 0.151 * cos__q_RF_HAA__) * sin__q_RF_HFE__) - (( 0.026 * cos__q_RF_HAA__) * cos__q_RF_HFE__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_LH_upperlegCOM::Type_fr_trunk_J_LH_upperlegCOM()
+{
+ (*this)(0,0) = - 1.0;
+ (*this)(0,1) = 0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+}
+
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_LH_upperlegCOM& iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_LH_upperlegCOM::update(const JState& jState) {
+ Scalar sin__q_LH_HAA__;
+ Scalar sin__q_LH_HFE__;
+ Scalar cos__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+
+ sin__q_LH_HAA__ = TRAIT::sin( jState(LH_HAA));
+ sin__q_LH_HFE__ = TRAIT::sin( jState(LH_HFE));
+ cos__q_LH_HAA__ = TRAIT::cos( jState(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( jState(LH_HFE));
+
+ (*this)(1,1) = cos__q_LH_HAA__;
+ (*this)(2,1) = - sin__q_LH_HAA__;
+ (*this)(3,1) = (( 0.026 * sin__q_LH_HFE__) - ( 0.151 * cos__q_LH_HFE__));
+ (*this)(4,0) = (((( 0.026 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.151 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) - ( 0.08 * cos__q_LH_HAA__));
+ (*this)(4,1) = ((( 0.151 * sin__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.026 * sin__q_LH_HAA__) * cos__q_LH_HFE__));
+ (*this)(5,0) = ((((- 0.026 * sin__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.151 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) + ( 0.08 * sin__q_LH_HAA__));
+ (*this)(5,1) = ((( 0.151 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.026 * cos__q_LH_HAA__) * cos__q_LH_HFE__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_RH_upperlegCOM::Type_fr_trunk_J_RH_upperlegCOM()
+{
+ (*this)(0,0) = 1.0;
+ (*this)(0,1) = 0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+}
+
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_RH_upperlegCOM& iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_RH_upperlegCOM::update(const JState& jState) {
+ Scalar sin__q_RH_HAA__;
+ Scalar sin__q_RH_HFE__;
+ Scalar cos__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+
+ sin__q_RH_HAA__ = TRAIT::sin( jState(RH_HAA));
+ sin__q_RH_HFE__ = TRAIT::sin( jState(RH_HFE));
+ cos__q_RH_HAA__ = TRAIT::cos( jState(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( jState(RH_HFE));
+
+ (*this)(1,1) = cos__q_RH_HAA__;
+ (*this)(2,1) = sin__q_RH_HAA__;
+ (*this)(3,1) = (( 0.026 * sin__q_RH_HFE__) - ( 0.151 * cos__q_RH_HFE__));
+ (*this)(4,0) = ((((- 0.026 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.151 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.08 * cos__q_RH_HAA__));
+ (*this)(4,1) = (((- 0.151 * sin__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.026 * sin__q_RH_HAA__) * cos__q_RH_HFE__));
+ (*this)(5,0) = ((((- 0.026 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.151 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.08 * sin__q_RH_HAA__));
+ (*this)(5,1) = ((( 0.151 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.026 * cos__q_RH_HAA__) * cos__q_RH_HFE__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_LF_lowerlegCOM::Type_fr_trunk_J_LF_lowerlegCOM()
+{
+ (*this)(0,0) = - 1.0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+}
+
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_LF_lowerlegCOM& iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_LF_lowerlegCOM::update(const JState& jState) {
+ Scalar sin__q_LF_HAA__;
+ Scalar sin__q_LF_HFE__;
+ Scalar sin__q_LF_KFE__;
+ Scalar cos__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+ Scalar cos__q_LF_KFE__;
+
+ sin__q_LF_HAA__ = TRAIT::sin( jState(LF_HAA));
+ sin__q_LF_HFE__ = TRAIT::sin( jState(LF_HFE));
+ sin__q_LF_KFE__ = TRAIT::sin( jState(LF_KFE));
+ cos__q_LF_HAA__ = TRAIT::cos( jState(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( jState(LF_HFE));
+ cos__q_LF_KFE__ = TRAIT::cos( jState(LF_KFE));
+
+ (*this)(1,1) = cos__q_LF_HAA__;
+ (*this)(1,2) = cos__q_LF_HAA__;
+ (*this)(2,1) = - sin__q_LF_HAA__;
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(3,1) = (((( 0.125 * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( 0.125 * cos__q_LF_HFE__) * cos__q_LF_KFE__)) - ( 0.35 * cos__q_LF_HFE__));
+ (*this)(3,2) = ((( 0.125 * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( 0.125 * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(4,0) = (((((( 0.125 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * sin__q_LF_KFE__) - ((( 0.125 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * cos__q_LF_KFE__)) - (( 0.35 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) - ( 0.08 * cos__q_LF_HAA__));
+ (*this)(4,1) = ((((( 0.125 * sin__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) + ((( 0.125 * sin__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__)) + (( 0.35 * sin__q_LF_HAA__) * sin__q_LF_HFE__));
+ (*this)(4,2) = (((( 0.125 * sin__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) + ((( 0.125 * sin__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(5,0) = ((((((- 0.125 * sin__q_LF_HAA__) * sin__q_LF_HFE__) * sin__q_LF_KFE__) + ((( 0.125 * sin__q_LF_HAA__) * cos__q_LF_HFE__) * cos__q_LF_KFE__)) + (( 0.35 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.08 * sin__q_LF_HAA__));
+ (*this)(5,1) = ((((( 0.125 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) + ((( 0.125 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__)) + (( 0.35 * cos__q_LF_HAA__) * sin__q_LF_HFE__));
+ (*this)(5,2) = (((( 0.125 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) + ((( 0.125 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_RF_lowerlegCOM::Type_fr_trunk_J_RF_lowerlegCOM()
+{
+ (*this)(0,0) = 1.0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+}
+
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_RF_lowerlegCOM& iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_RF_lowerlegCOM::update(const JState& jState) {
+ Scalar sin__q_RF_HAA__;
+ Scalar sin__q_RF_HFE__;
+ Scalar sin__q_RF_KFE__;
+ Scalar cos__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+ Scalar cos__q_RF_KFE__;
+
+ sin__q_RF_HAA__ = TRAIT::sin( jState(RF_HAA));
+ sin__q_RF_HFE__ = TRAIT::sin( jState(RF_HFE));
+ sin__q_RF_KFE__ = TRAIT::sin( jState(RF_KFE));
+ cos__q_RF_HAA__ = TRAIT::cos( jState(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( jState(RF_HFE));
+ cos__q_RF_KFE__ = TRAIT::cos( jState(RF_KFE));
+
+ (*this)(1,1) = cos__q_RF_HAA__;
+ (*this)(1,2) = cos__q_RF_HAA__;
+ (*this)(2,1) = sin__q_RF_HAA__;
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(3,1) = ((((( 0.125 * sin__q_RF_HFE__) + ( 0.001 * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + ((( 0.001 * sin__q_RF_HFE__) - ( 0.125 * cos__q_RF_HFE__)) * cos__q_RF_KFE__)) - ( 0.35 * cos__q_RF_HFE__));
+ (*this)(3,2) = (((( 0.125 * sin__q_RF_HFE__) + ( 0.001 * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + ((( 0.001 * sin__q_RF_HFE__) - ( 0.125 * cos__q_RF_HFE__)) * cos__q_RF_KFE__));
+ (*this)(4,0) = (((((((- 0.125 * cos__q_RF_HAA__) * sin__q_RF_HFE__) - (( 0.001 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + (((( 0.125 * cos__q_RF_HAA__) * cos__q_RF_HFE__) - (( 0.001 * cos__q_RF_HAA__) * sin__q_RF_HFE__)) * cos__q_RF_KFE__)) + (( 0.35 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.08 * cos__q_RF_HAA__));
+ (*this)(4,1) = (((((( 0.001 * sin__q_RF_HAA__) * sin__q_RF_HFE__) - (( 0.125 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + ((((- 0.125 * sin__q_RF_HAA__) * sin__q_RF_HFE__) - (( 0.001 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) * cos__q_RF_KFE__)) - (( 0.35 * sin__q_RF_HAA__) * sin__q_RF_HFE__));
+ (*this)(4,2) = ((((( 0.001 * sin__q_RF_HAA__) * sin__q_RF_HFE__) - (( 0.125 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + ((((- 0.125 * sin__q_RF_HAA__) * sin__q_RF_HFE__) - (( 0.001 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) * cos__q_RF_KFE__));
+ (*this)(5,0) = (((((((- 0.125 * sin__q_RF_HAA__) * sin__q_RF_HFE__) - (( 0.001 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + (((( 0.125 * sin__q_RF_HAA__) * cos__q_RF_HFE__) - (( 0.001 * sin__q_RF_HAA__) * sin__q_RF_HFE__)) * cos__q_RF_KFE__)) + (( 0.35 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.08 * sin__q_RF_HAA__));
+ (*this)(5,1) = (((((( 0.125 * cos__q_RF_HAA__) * cos__q_RF_HFE__) - (( 0.001 * cos__q_RF_HAA__) * sin__q_RF_HFE__)) * sin__q_RF_KFE__) + (((( 0.125 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.001 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) * cos__q_RF_KFE__)) + (( 0.35 * cos__q_RF_HAA__) * sin__q_RF_HFE__));
+ (*this)(5,2) = ((((( 0.125 * cos__q_RF_HAA__) * cos__q_RF_HFE__) - (( 0.001 * cos__q_RF_HAA__) * sin__q_RF_HFE__)) * sin__q_RF_KFE__) + (((( 0.125 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.001 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) * cos__q_RF_KFE__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_LH_lowerlegCOM::Type_fr_trunk_J_LH_lowerlegCOM()
+{
+ (*this)(0,0) = - 1.0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+}
+
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_LH_lowerlegCOM& iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_LH_lowerlegCOM::update(const JState& jState) {
+ Scalar sin__q_LH_HAA__;
+ Scalar sin__q_LH_HFE__;
+ Scalar sin__q_LH_KFE__;
+ Scalar cos__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+ Scalar cos__q_LH_KFE__;
+
+ sin__q_LH_HAA__ = TRAIT::sin( jState(LH_HAA));
+ sin__q_LH_HFE__ = TRAIT::sin( jState(LH_HFE));
+ sin__q_LH_KFE__ = TRAIT::sin( jState(LH_KFE));
+ cos__q_LH_HAA__ = TRAIT::cos( jState(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( jState(LH_HFE));
+ cos__q_LH_KFE__ = TRAIT::cos( jState(LH_KFE));
+
+ (*this)(1,1) = cos__q_LH_HAA__;
+ (*this)(1,2) = cos__q_LH_HAA__;
+ (*this)(2,1) = - sin__q_LH_HAA__;
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(3,1) = ((((( 0.125 * sin__q_LH_HFE__) - ( 0.001 * cos__q_LH_HFE__)) * sin__q_LH_KFE__) + (((- 0.001 * sin__q_LH_HFE__) - ( 0.125 * cos__q_LH_HFE__)) * cos__q_LH_KFE__)) - ( 0.35 * cos__q_LH_HFE__));
+ (*this)(3,2) = (((( 0.125 * sin__q_LH_HFE__) - ( 0.001 * cos__q_LH_HFE__)) * sin__q_LH_KFE__) + (((- 0.001 * sin__q_LH_HFE__) - ( 0.125 * cos__q_LH_HFE__)) * cos__q_LH_KFE__));
+ (*this)(4,0) = ((((((( 0.125 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.001 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) * sin__q_LH_KFE__) + ((((- 0.001 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.125 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) * cos__q_LH_KFE__)) - (( 0.35 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) - ( 0.08 * cos__q_LH_HAA__));
+ (*this)(4,1) = (((((( 0.001 * sin__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.125 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) * sin__q_LH_KFE__) + (((( 0.125 * sin__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.001 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) * cos__q_LH_KFE__)) + (( 0.35 * sin__q_LH_HAA__) * sin__q_LH_HFE__));
+ (*this)(4,2) = ((((( 0.001 * sin__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.125 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) * sin__q_LH_KFE__) + (((( 0.125 * sin__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.001 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) * cos__q_LH_KFE__));
+ (*this)(5,0) = ((((((( 0.001 * sin__q_LH_HAA__) * cos__q_LH_HFE__) - (( 0.125 * sin__q_LH_HAA__) * sin__q_LH_HFE__)) * sin__q_LH_KFE__) + (((( 0.001 * sin__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.125 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) * cos__q_LH_KFE__)) + (( 0.35 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) + ( 0.08 * sin__q_LH_HAA__));
+ (*this)(5,1) = (((((( 0.001 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.125 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) * sin__q_LH_KFE__) + (((( 0.125 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.001 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) * cos__q_LH_KFE__)) + (( 0.35 * cos__q_LH_HAA__) * sin__q_LH_HFE__));
+ (*this)(5,2) = ((((( 0.001 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.125 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) * sin__q_LH_KFE__) + (((( 0.125 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.001 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) * cos__q_LH_KFE__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_RH_lowerlegCOM::Type_fr_trunk_J_RH_lowerlegCOM()
+{
+ (*this)(0,0) = 1.0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+}
+
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_RH_lowerlegCOM& iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_RH_lowerlegCOM::update(const JState& jState) {
+ Scalar sin__q_RH_HAA__;
+ Scalar sin__q_RH_HFE__;
+ Scalar sin__q_RH_KFE__;
+ Scalar cos__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+ Scalar cos__q_RH_KFE__;
+
+ sin__q_RH_HAA__ = TRAIT::sin( jState(RH_HAA));
+ sin__q_RH_HFE__ = TRAIT::sin( jState(RH_HFE));
+ sin__q_RH_KFE__ = TRAIT::sin( jState(RH_KFE));
+ cos__q_RH_HAA__ = TRAIT::cos( jState(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( jState(RH_HFE));
+ cos__q_RH_KFE__ = TRAIT::cos( jState(RH_KFE));
+
+ (*this)(1,1) = cos__q_RH_HAA__;
+ (*this)(1,2) = cos__q_RH_HAA__;
+ (*this)(2,1) = sin__q_RH_HAA__;
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(3,1) = ((((( 0.125 * sin__q_RH_HFE__) - ( 0.001 * cos__q_RH_HFE__)) * sin__q_RH_KFE__) + (((- 0.001 * sin__q_RH_HFE__) - ( 0.125 * cos__q_RH_HFE__)) * cos__q_RH_KFE__)) - ( 0.35 * cos__q_RH_HFE__));
+ (*this)(3,2) = (((( 0.125 * sin__q_RH_HFE__) - ( 0.001 * cos__q_RH_HFE__)) * sin__q_RH_KFE__) + (((- 0.001 * sin__q_RH_HFE__) - ( 0.125 * cos__q_RH_HFE__)) * cos__q_RH_KFE__));
+ (*this)(4,0) = ((((((( 0.001 * cos__q_RH_HAA__) * cos__q_RH_HFE__) - (( 0.125 * cos__q_RH_HAA__) * sin__q_RH_HFE__)) * sin__q_RH_KFE__) + (((( 0.001 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.125 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) * cos__q_RH_KFE__)) + (( 0.35 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.08 * cos__q_RH_HAA__));
+ (*this)(4,1) = ((((((- 0.001 * sin__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.125 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) * sin__q_RH_KFE__) + (((( 0.001 * sin__q_RH_HAA__) * cos__q_RH_HFE__) - (( 0.125 * sin__q_RH_HAA__) * sin__q_RH_HFE__)) * cos__q_RH_KFE__)) - (( 0.35 * sin__q_RH_HAA__) * sin__q_RH_HFE__));
+ (*this)(4,2) = (((((- 0.001 * sin__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.125 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) * sin__q_RH_KFE__) + (((( 0.001 * sin__q_RH_HAA__) * cos__q_RH_HFE__) - (( 0.125 * sin__q_RH_HAA__) * sin__q_RH_HFE__)) * cos__q_RH_KFE__));
+ (*this)(5,0) = ((((((( 0.001 * sin__q_RH_HAA__) * cos__q_RH_HFE__) - (( 0.125 * sin__q_RH_HAA__) * sin__q_RH_HFE__)) * sin__q_RH_KFE__) + (((( 0.001 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.125 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) * cos__q_RH_KFE__)) + (( 0.35 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.08 * sin__q_RH_HAA__));
+ (*this)(5,1) = (((((( 0.001 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.125 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) * sin__q_RH_KFE__) + (((( 0.125 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.001 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) * cos__q_RH_KFE__)) + (( 0.35 * cos__q_RH_HAA__) * sin__q_RH_HFE__));
+ (*this)(5,2) = ((((( 0.001 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.125 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) * sin__q_RH_KFE__) + (((( 0.125 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.001 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) * cos__q_RH_KFE__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_fr_LF_foot::Type_fr_trunk_J_fr_LF_foot()
+{
+ (*this)(0,0) = - 1.0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+}
+
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_fr_LF_foot& iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_fr_LF_foot::update(const JState& jState) {
+ Scalar sin__q_LF_HAA__;
+ Scalar sin__q_LF_HFE__;
+ Scalar sin__q_LF_KFE__;
+ Scalar cos__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+ Scalar cos__q_LF_KFE__;
+
+ sin__q_LF_HAA__ = TRAIT::sin( jState(LF_HAA));
+ sin__q_LF_HFE__ = TRAIT::sin( jState(LF_HFE));
+ sin__q_LF_KFE__ = TRAIT::sin( jState(LF_KFE));
+ cos__q_LF_HAA__ = TRAIT::cos( jState(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( jState(LF_HFE));
+ cos__q_LF_KFE__ = TRAIT::cos( jState(LF_KFE));
+
+ (*this)(1,1) = cos__q_LF_HAA__;
+ (*this)(1,2) = cos__q_LF_HAA__;
+ (*this)(2,1) = - sin__q_LF_HAA__;
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(3,1) = (((( 0.33 * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( 0.33 * cos__q_LF_HFE__) * cos__q_LF_KFE__)) - ( 0.35 * cos__q_LF_HFE__));
+ (*this)(3,2) = ((( 0.33 * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( 0.33 * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(4,0) = (((((( 0.33 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * sin__q_LF_KFE__) - ((( 0.33 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * cos__q_LF_KFE__)) - (( 0.35 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) - ( 0.08 * cos__q_LF_HAA__));
+ (*this)(4,1) = ((((( 0.33 * sin__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) + ((( 0.33 * sin__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__)) + (( 0.35 * sin__q_LF_HAA__) * sin__q_LF_HFE__));
+ (*this)(4,2) = (((( 0.33 * sin__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) + ((( 0.33 * sin__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(5,0) = ((((((- 0.33 * sin__q_LF_HAA__) * sin__q_LF_HFE__) * sin__q_LF_KFE__) + ((( 0.33 * sin__q_LF_HAA__) * cos__q_LF_HFE__) * cos__q_LF_KFE__)) + (( 0.35 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.08 * sin__q_LF_HAA__));
+ (*this)(5,1) = ((((( 0.33 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) + ((( 0.33 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__)) + (( 0.35 * cos__q_LF_HAA__) * sin__q_LF_HFE__));
+ (*this)(5,2) = (((( 0.33 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) + ((( 0.33 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_fr_RF_foot::Type_fr_trunk_J_fr_RF_foot()
+{
+ (*this)(0,0) = 1.0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+}
+
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_fr_RF_foot& iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_fr_RF_foot::update(const JState& jState) {
+ Scalar sin__q_RF_HAA__;
+ Scalar sin__q_RF_HFE__;
+ Scalar sin__q_RF_KFE__;
+ Scalar cos__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+ Scalar cos__q_RF_KFE__;
+
+ sin__q_RF_HAA__ = TRAIT::sin( jState(RF_HAA));
+ sin__q_RF_HFE__ = TRAIT::sin( jState(RF_HFE));
+ sin__q_RF_KFE__ = TRAIT::sin( jState(RF_KFE));
+ cos__q_RF_HAA__ = TRAIT::cos( jState(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( jState(RF_HFE));
+ cos__q_RF_KFE__ = TRAIT::cos( jState(RF_KFE));
+
+ (*this)(1,1) = cos__q_RF_HAA__;
+ (*this)(1,2) = cos__q_RF_HAA__;
+ (*this)(2,1) = sin__q_RF_HAA__;
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(3,1) = (((( 0.33 * sin__q_RF_HFE__) * sin__q_RF_KFE__) - (( 0.33 * cos__q_RF_HFE__) * cos__q_RF_KFE__)) - ( 0.35 * cos__q_RF_HFE__));
+ (*this)(3,2) = ((( 0.33 * sin__q_RF_HFE__) * sin__q_RF_KFE__) - (( 0.33 * cos__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(4,0) = ((((((- 0.33 * cos__q_RF_HAA__) * sin__q_RF_HFE__) * sin__q_RF_KFE__) + ((( 0.33 * cos__q_RF_HAA__) * cos__q_RF_HFE__) * cos__q_RF_KFE__)) + (( 0.35 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.08 * cos__q_RF_HAA__));
+ (*this)(4,1) = (((((- 0.33 * sin__q_RF_HAA__) * cos__q_RF_HFE__) * sin__q_RF_KFE__) - ((( 0.33 * sin__q_RF_HAA__) * sin__q_RF_HFE__) * cos__q_RF_KFE__)) - (( 0.35 * sin__q_RF_HAA__) * sin__q_RF_HFE__));
+ (*this)(4,2) = ((((- 0.33 * sin__q_RF_HAA__) * cos__q_RF_HFE__) * sin__q_RF_KFE__) - ((( 0.33 * sin__q_RF_HAA__) * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(5,0) = ((((((- 0.33 * sin__q_RF_HAA__) * sin__q_RF_HFE__) * sin__q_RF_KFE__) + ((( 0.33 * sin__q_RF_HAA__) * cos__q_RF_HFE__) * cos__q_RF_KFE__)) + (( 0.35 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.08 * sin__q_RF_HAA__));
+ (*this)(5,1) = ((((( 0.33 * cos__q_RF_HAA__) * cos__q_RF_HFE__) * sin__q_RF_KFE__) + ((( 0.33 * cos__q_RF_HAA__) * sin__q_RF_HFE__) * cos__q_RF_KFE__)) + (( 0.35 * cos__q_RF_HAA__) * sin__q_RF_HFE__));
+ (*this)(5,2) = (((( 0.33 * cos__q_RF_HAA__) * cos__q_RF_HFE__) * sin__q_RF_KFE__) + ((( 0.33 * cos__q_RF_HAA__) * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_fr_LH_foot::Type_fr_trunk_J_fr_LH_foot()
+{
+ (*this)(0,0) = - 1.0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+}
+
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_fr_LH_foot& iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_fr_LH_foot::update(const JState& jState) {
+ Scalar sin__q_LH_HAA__;
+ Scalar sin__q_LH_HFE__;
+ Scalar sin__q_LH_KFE__;
+ Scalar cos__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+ Scalar cos__q_LH_KFE__;
+
+ sin__q_LH_HAA__ = TRAIT::sin( jState(LH_HAA));
+ sin__q_LH_HFE__ = TRAIT::sin( jState(LH_HFE));
+ sin__q_LH_KFE__ = TRAIT::sin( jState(LH_KFE));
+ cos__q_LH_HAA__ = TRAIT::cos( jState(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( jState(LH_HFE));
+ cos__q_LH_KFE__ = TRAIT::cos( jState(LH_KFE));
+
+ (*this)(1,1) = cos__q_LH_HAA__;
+ (*this)(1,2) = cos__q_LH_HAA__;
+ (*this)(2,1) = - sin__q_LH_HAA__;
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(3,1) = (((( 0.33 * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( 0.33 * cos__q_LH_HFE__) * cos__q_LH_KFE__)) - ( 0.35 * cos__q_LH_HFE__));
+ (*this)(3,2) = ((( 0.33 * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( 0.33 * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(4,0) = (((((( 0.33 * cos__q_LH_HAA__) * sin__q_LH_HFE__) * sin__q_LH_KFE__) - ((( 0.33 * cos__q_LH_HAA__) * cos__q_LH_HFE__) * cos__q_LH_KFE__)) - (( 0.35 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) - ( 0.08 * cos__q_LH_HAA__));
+ (*this)(4,1) = ((((( 0.33 * sin__q_LH_HAA__) * cos__q_LH_HFE__) * sin__q_LH_KFE__) + ((( 0.33 * sin__q_LH_HAA__) * sin__q_LH_HFE__) * cos__q_LH_KFE__)) + (( 0.35 * sin__q_LH_HAA__) * sin__q_LH_HFE__));
+ (*this)(4,2) = (((( 0.33 * sin__q_LH_HAA__) * cos__q_LH_HFE__) * sin__q_LH_KFE__) + ((( 0.33 * sin__q_LH_HAA__) * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(5,0) = ((((((- 0.33 * sin__q_LH_HAA__) * sin__q_LH_HFE__) * sin__q_LH_KFE__) + ((( 0.33 * sin__q_LH_HAA__) * cos__q_LH_HFE__) * cos__q_LH_KFE__)) + (( 0.35 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) + ( 0.08 * sin__q_LH_HAA__));
+ (*this)(5,1) = ((((( 0.33 * cos__q_LH_HAA__) * cos__q_LH_HFE__) * sin__q_LH_KFE__) + ((( 0.33 * cos__q_LH_HAA__) * sin__q_LH_HFE__) * cos__q_LH_KFE__)) + (( 0.35 * cos__q_LH_HAA__) * sin__q_LH_HFE__));
+ (*this)(5,2) = (((( 0.33 * cos__q_LH_HAA__) * cos__q_LH_HFE__) * sin__q_LH_KFE__) + ((( 0.33 * cos__q_LH_HAA__) * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_fr_RH_foot::Type_fr_trunk_J_fr_RH_foot()
+{
+ (*this)(0,0) = 1.0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+}
+
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_fr_RH_foot& iit::TestHyQ::tpl::Jacobians<TRAIT>::Type_fr_trunk_J_fr_RH_foot::update(const JState& jState) {
+ Scalar sin__q_RH_HAA__;
+ Scalar sin__q_RH_HFE__;
+ Scalar sin__q_RH_KFE__;
+ Scalar cos__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+ Scalar cos__q_RH_KFE__;
+
+ sin__q_RH_HAA__ = TRAIT::sin( jState(RH_HAA));
+ sin__q_RH_HFE__ = TRAIT::sin( jState(RH_HFE));
+ sin__q_RH_KFE__ = TRAIT::sin( jState(RH_KFE));
+ cos__q_RH_HAA__ = TRAIT::cos( jState(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( jState(RH_HFE));
+ cos__q_RH_KFE__ = TRAIT::cos( jState(RH_KFE));
+
+ (*this)(1,1) = cos__q_RH_HAA__;
+ (*this)(1,2) = cos__q_RH_HAA__;
+ (*this)(2,1) = sin__q_RH_HAA__;
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(3,1) = (((( 0.33 * sin__q_RH_HFE__) * sin__q_RH_KFE__) - (( 0.33 * cos__q_RH_HFE__) * cos__q_RH_KFE__)) - ( 0.35 * cos__q_RH_HFE__));
+ (*this)(3,2) = ((( 0.33 * sin__q_RH_HFE__) * sin__q_RH_KFE__) - (( 0.33 * cos__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(4,0) = ((((((- 0.33 * cos__q_RH_HAA__) * sin__q_RH_HFE__) * sin__q_RH_KFE__) + ((( 0.33 * cos__q_RH_HAA__) * cos__q_RH_HFE__) * cos__q_RH_KFE__)) + (( 0.35 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.08 * cos__q_RH_HAA__));
+ (*this)(4,1) = (((((- 0.33 * sin__q_RH_HAA__) * cos__q_RH_HFE__) * sin__q_RH_KFE__) - ((( 0.33 * sin__q_RH_HAA__) * sin__q_RH_HFE__) * cos__q_RH_KFE__)) - (( 0.35 * sin__q_RH_HAA__) * sin__q_RH_HFE__));
+ (*this)(4,2) = ((((- 0.33 * sin__q_RH_HAA__) * cos__q_RH_HFE__) * sin__q_RH_KFE__) - ((( 0.33 * sin__q_RH_HAA__) * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(5,0) = ((((((- 0.33 * sin__q_RH_HAA__) * sin__q_RH_HFE__) * sin__q_RH_KFE__) + ((( 0.33 * sin__q_RH_HAA__) * cos__q_RH_HFE__) * cos__q_RH_KFE__)) + (( 0.35 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.08 * sin__q_RH_HAA__));
+ (*this)(5,1) = ((((( 0.33 * cos__q_RH_HAA__) * cos__q_RH_HFE__) * sin__q_RH_KFE__) + ((( 0.33 * cos__q_RH_HAA__) * sin__q_RH_HFE__) * cos__q_RH_KFE__)) + (( 0.35 * cos__q_RH_HAA__) * sin__q_RH_HFE__));
+ (*this)(5,2) = (((( 0.33 * cos__q_RH_HAA__) * cos__q_RH_HFE__) * sin__q_RH_KFE__) + ((( 0.33 * cos__q_RH_HAA__) * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ return *this;
+}
diff --git a/ct_rbd/test/models/testhyq/generated/joint_data_map.h b/ct_rbd/test/models/testhyq/generated/joint_data_map.h
new file mode 100644
index 0000000..7506c62
--- /dev/null
+++ b/ct_rbd/test/models/testhyq/generated/joint_data_map.h
@@ -0,0 +1,130 @@
+#ifndef IIT_TESTHYQ_JOINT_DATA_MAP_H_
+#define IIT_TESTHYQ_JOINT_DATA_MAP_H_
+
+#include "declarations.h"
+
+namespace iit {
+namespace TestHyQ {
+
+/**
+ * A very simple container to associate a generic data item to each joint
+ */
+template<typename T> class JointDataMap {
+private:
+ T data[jointsCount];
+public:
+ JointDataMap() {};
+ JointDataMap(const T& defaultValue);
+ JointDataMap(const JointDataMap& rhs);
+ JointDataMap& operator=(const JointDataMap& rhs);
+ JointDataMap& operator=(const T& rhs);
+ T& operator[](JointIdentifiers which);
+ const T& operator[](JointIdentifiers which) const;
+private:
+ void copydata(const JointDataMap& rhs);
+ void assigndata(const T& rhs);
+};
+
+template<typename T> inline
+JointDataMap<T>::JointDataMap(const T& value) {
+ assigndata(value);
+}
+
+template<typename T> inline
+JointDataMap<T>::JointDataMap(const JointDataMap& rhs)
+{
+ copydata(rhs);
+}
+
+template<typename T> inline
+JointDataMap<T>& JointDataMap<T>::operator=(const JointDataMap& rhs)
+{
+ if(&rhs != this) {
+ copydata(rhs);
+ }
+ return *this;
+}
+
+template<typename T> inline
+JointDataMap<T>& JointDataMap<T>::operator=(const T& value)
+{
+ assigndata(value);
+ return *this;
+}
+
+template<typename T> inline
+T& JointDataMap<T>::operator[](JointIdentifiers j) {
+ return data[j];
+}
+
+template<typename T> inline
+const T& JointDataMap<T>::operator[](JointIdentifiers j) const {
+ return data[j];
+}
+
+template<typename T> inline
+void JointDataMap<T>::copydata(const JointDataMap& rhs) {
+ data[LF_HAA] = rhs[LF_HAA];
+ data[LF_HFE] = rhs[LF_HFE];
+ data[LF_KFE] = rhs[LF_KFE];
+ data[RF_HAA] = rhs[RF_HAA];
+ data[RF_HFE] = rhs[RF_HFE];
+ data[RF_KFE] = rhs[RF_KFE];
+ data[LH_HAA] = rhs[LH_HAA];
+ data[LH_HFE] = rhs[LH_HFE];
+ data[LH_KFE] = rhs[LH_KFE];
+ data[RH_HAA] = rhs[RH_HAA];
+ data[RH_HFE] = rhs[RH_HFE];
+ data[RH_KFE] = rhs[RH_KFE];
+}
+
+template<typename T> inline
+void JointDataMap<T>::assigndata(const T& value) {
+ data[LF_HAA] = value;
+ data[LF_HFE] = value;
+ data[LF_KFE] = value;
+ data[RF_HAA] = value;
+ data[RF_HFE] = value;
+ data[RF_KFE] = value;
+ data[LH_HAA] = value;
+ data[LH_HFE] = value;
+ data[LH_KFE] = value;
+ data[RH_HAA] = value;
+ data[RH_HFE] = value;
+ data[RH_KFE] = value;
+}
+
+template<typename T> inline
+std::ostream& operator<<(std::ostream& out, const JointDataMap<T>& map) {
+ out
+ << " LF_HAA = "
+ << map[LF_HAA]
+ << " LF_HFE = "
+ << map[LF_HFE]
+ << " LF_KFE = "
+ << map[LF_KFE]
+ << " RF_HAA = "
+ << map[RF_HAA]
+ << " RF_HFE = "
+ << map[RF_HFE]
+ << " RF_KFE = "
+ << map[RF_KFE]
+ << " LH_HAA = "
+ << map[LH_HAA]
+ << " LH_HFE = "
+ << map[LH_HFE]
+ << " LH_KFE = "
+ << map[LH_KFE]
+ << " RH_HAA = "
+ << map[RH_HAA]
+ << " RH_HFE = "
+ << map[RH_HFE]
+ << " RH_KFE = "
+ << map[RH_KFE]
+ ;
+ return out;
+}
+
+}
+}
+#endif
diff --git a/ct_rbd/test/models/testhyq/generated/jsim.h b/ct_rbd/test/models/testhyq/generated/jsim.h
new file mode 100644
index 0000000..baf03f0
--- /dev/null
+++ b/ct_rbd/test/models/testhyq/generated/jsim.h
@@ -0,0 +1,162 @@
+#ifndef IIT_TESTHYQ_JSIM_H_
+#define IIT_TESTHYQ_JSIM_H_
+
+#include <iit/rbd/rbd.h>
+#include <iit/rbd/StateDependentMatrix.h>
+
+#include "declarations.h"
+#include "transforms.h"
+#include "inertia_properties.h"
+#include <iit/rbd/robcogen_commons.h>
+#include <iit/rbd/traits/DoubleTrait.h>
+
+
+namespace iit {
+namespace TestHyQ {
+namespace dyn {
+
+namespace tpl {
+
+/**
+ * The type of the Joint Space Inertia Matrix (JSIM) of the robot TestHyQ.
+ */
+template <typename TRAIT>
+class JSIM : public iit::rbd::StateDependentMatrix<iit::TestHyQ::tpl::JointState<typename TRAIT::Scalar>, 18, 18, JSIM<TRAIT>>
+{
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ private:
+ typedef iit::rbd::StateDependentMatrix<iit::TestHyQ::tpl::JointState<typename TRAIT::Scalar>, 18, 18, JSIM<TRAIT>> Base;
+ public:
+ typedef typename TRAIT::Scalar Scalar;
+ typedef typename iit::TestHyQ::tpl::JointState<Scalar> JointState;
+ typedef iit::rbd::Core<Scalar> CoreS;
+ typedef typename Base::Index Index;
+ typedef typename iit::rbd::PlainMatrix<Scalar, 18, 18> MatrixType;
+ /** The type of the F sub-block of the floating-base JSIM */
+ typedef const Eigen::Block<const MatrixType,6,12> BlockF_t;
+ /** The type of the fixed-base sub-block of the JSIM */
+ typedef const Eigen::Block<const MatrixType,12,12> BlockFixedBase_t;
+ typedef InertiaProperties<TRAIT> IProperties;
+ typedef iit::TestHyQ::tpl::ForceTransforms<TRAIT> FTransforms;
+ typedef iit::rbd::tpl::InertiaMatrixDense<Scalar> InertiaMatrix;
+
+ public:
+ JSIM(IProperties&, FTransforms&);
+ ~JSIM() {}
+
+ const JSIM& update(const JointState&);
+
+
+ /**
+ * Computes and saves the matrix L of the L^T L factorization of this JSIM.
+ */
+ void computeL();
+ /**
+ * Computes and saves the inverse of this JSIM.
+ * This function assumes that computeL() has been called already, since it
+ * uses L to compute the inverse. The algorithm takes advantage of the branch
+ * induced sparsity of the robot, if any.
+ */
+ void computeInverse();
+ /**
+ * Returns an unmodifiable reference to the matrix L. See also computeL()
+ */
+ const MatrixType& getL() const;
+ /**
+ * Returns an unmodifiable reference to the inverse of this JSIM
+ */
+ const MatrixType& getInverse() const;
+
+ /**
+ * The spatial composite-inertia tensor of the robot base,
+ * ie the inertia of the whole robot for the current configuration.
+ * According to the convention of this class about the layout of the
+ * floating-base JSIM, this tensor is the 6x6 upper left corner of
+ * the JSIM itself.
+ * \return the 6x6 InertiaMatrix that correspond to the spatial inertia
+ * tensor of the whole robot, according to the last joints configuration
+ * used to update this JSIM
+ */
+ const InertiaMatrix& getWholeBodyInertia() const;
+ /**
+ * The matrix that maps accelerations in the actual joints of the robot
+ * to the spatial force acting on the floating-base of the robot.
+ * This matrix is the F sub-block of the JSIM in Featherstone's notation.
+ * \return the 6x12 upper right block of this JSIM
+ */
+ const BlockF_t getF() const;
+ /**
+ * The submatrix of this JSIM related only to the actual joints of the
+ * robot (as for a fixed-base robot).
+ * This matrix is the H sub-block of the JSIM in Featherstone's notation.
+ * \return the 12x12 lower right block of this JSIM,
+ * which correspond to the fixed-base JSIM
+ */
+ const BlockFixedBase_t getFixedBaseBlock() const;
+ protected:
+ /**
+ * Computes and saves the inverse of the matrix L. See also computeL()
+ */
+ void computeLInverse();
+ private:
+ IProperties& linkInertias;
+ FTransforms* frcTransf;
+
+ // The composite-inertia tensor for each link
+ InertiaMatrix trunk_Ic;
+ InertiaMatrix LF_hipassembly_Ic;
+ InertiaMatrix LF_upperleg_Ic;
+ const InertiaMatrix& LF_lowerleg_Ic;
+ InertiaMatrix RF_hipassembly_Ic;
+ InertiaMatrix RF_upperleg_Ic;
+ const InertiaMatrix& RF_lowerleg_Ic;
+ InertiaMatrix LH_hipassembly_Ic;
+ InertiaMatrix LH_upperleg_Ic;
+ const InertiaMatrix& LH_lowerleg_Ic;
+ InertiaMatrix RH_hipassembly_Ic;
+ InertiaMatrix RH_upperleg_Ic;
+ const InertiaMatrix& RH_lowerleg_Ic;
+ InertiaMatrix Ic_spare;
+
+ MatrixType L;
+ MatrixType Linv;
+ MatrixType inverse;
+};
+
+template <typename TRAIT>
+inline const typename JSIM<TRAIT>::MatrixType& JSIM<TRAIT>::getL() const {
+ return L;
+}
+
+template <typename TRAIT>
+inline const typename JSIM<TRAIT>::MatrixType& JSIM<TRAIT>::getInverse() const {
+ return inverse;
+}
+
+template <typename TRAIT>
+inline const typename JSIM<TRAIT>::InertiaMatrix& JSIM<TRAIT>::getWholeBodyInertia() const {
+ return trunk_Ic;
+}
+
+template <typename TRAIT>
+inline const typename JSIM<TRAIT>::BlockF_t JSIM<TRAIT>::getF() const {
+ return JSIM<TRAIT>:: template block<6,12>(0,6);
+}
+
+template <typename TRAIT>
+inline const typename JSIM<TRAIT>::BlockFixedBase_t JSIM<TRAIT>::getFixedBaseBlock() const{
+ return JSIM<TRAIT>:: template block<12,12>(6,6);
+}
+
+}
+
+typedef tpl::JSIM<rbd::DoubleTrait> JSIM;
+
+}
+}
+}
+
+#include "jsim.impl.h"
+
+#endif
diff --git a/ct_rbd/test/models/testhyq/generated/jsim.impl.h b/ct_rbd/test/models/testhyq/generated/jsim.impl.h
new file mode 100644
index 0000000..5fb0c8f
--- /dev/null
+++ b/ct_rbd/test/models/testhyq/generated/jsim.impl.h
@@ -0,0 +1,344 @@
+
+
+//Implementation of default constructor
+template <typename TRAIT>
+iit::TestHyQ::dyn::tpl::JSIM<TRAIT>::JSIM(IProperties& inertiaProperties, FTransforms& forceTransforms) :
+ linkInertias(inertiaProperties),
+ frcTransf( &forceTransforms ),
+ LF_lowerleg_Ic(linkInertias.getTensor_LF_lowerleg()),
+ RF_lowerleg_Ic(linkInertias.getTensor_RF_lowerleg()),
+ LH_lowerleg_Ic(linkInertias.getTensor_LH_lowerleg()),
+ RH_lowerleg_Ic(linkInertias.getTensor_RH_lowerleg())
+{
+ //Initialize the matrix itself
+ this->setZero();
+}
+
+#define DATA tpl::JSIM<TRAIT>::operator()
+#define Fcol(j) (tpl::JSIM<TRAIT>:: template block<6,1>(0,(j)+6))
+#define F(i,j) DATA((i),(j)+6)
+
+template <typename TRAIT>
+const typename iit::TestHyQ::dyn::tpl::JSIM<TRAIT>& iit::TestHyQ::dyn::tpl::JSIM<TRAIT>::update(const JointState& state) {
+
+ // Precomputes only once the coordinate transforms:
+ frcTransf -> fr_RH_upperleg_X_fr_RH_lowerleg(state);
+ frcTransf -> fr_RH_hipassembly_X_fr_RH_upperleg(state);
+ frcTransf -> fr_trunk_X_fr_RH_hipassembly(state);
+ frcTransf -> fr_LH_upperleg_X_fr_LH_lowerleg(state);
+ frcTransf -> fr_LH_hipassembly_X_fr_LH_upperleg(state);
+ frcTransf -> fr_trunk_X_fr_LH_hipassembly(state);
+ frcTransf -> fr_RF_upperleg_X_fr_RF_lowerleg(state);
+ frcTransf -> fr_RF_hipassembly_X_fr_RF_upperleg(state);
+ frcTransf -> fr_trunk_X_fr_RF_hipassembly(state);
+ frcTransf -> fr_LF_upperleg_X_fr_LF_lowerleg(state);
+ frcTransf -> fr_LF_hipassembly_X_fr_LF_upperleg(state);
+ frcTransf -> fr_trunk_X_fr_LF_hipassembly(state);
+
+ // Initializes the composite inertia tensors
+ trunk_Ic = linkInertias.getTensor_trunk();
+ LF_hipassembly_Ic = linkInertias.getTensor_LF_hipassembly();
+ LF_upperleg_Ic = linkInertias.getTensor_LF_upperleg();
+ RF_hipassembly_Ic = linkInertias.getTensor_RF_hipassembly();
+ RF_upperleg_Ic = linkInertias.getTensor_RF_upperleg();
+ LH_hipassembly_Ic = linkInertias.getTensor_LH_hipassembly();
+ LH_upperleg_Ic = linkInertias.getTensor_LH_upperleg();
+ RH_hipassembly_Ic = linkInertias.getTensor_RH_hipassembly();
+ RH_upperleg_Ic = linkInertias.getTensor_RH_upperleg();
+
+ // "Bottom-up" loop to update the inertia-composite property of each link, for the current configuration
+
+ // Link RH_lowerleg:
+ iit::rbd::transformInertia<Scalar>(RH_lowerleg_Ic, frcTransf -> fr_RH_upperleg_X_fr_RH_lowerleg, Ic_spare);
+ RH_upperleg_Ic += Ic_spare;
+
+ Fcol(RH_KFE) = RH_lowerleg_Ic.col(iit::rbd::AZ);
+ DATA(RH_KFE+6, RH_KFE+6) = Fcol(RH_KFE)(iit::rbd::AZ);
+
+ Fcol(RH_KFE) = frcTransf -> fr_RH_upperleg_X_fr_RH_lowerleg * Fcol(RH_KFE);
+ DATA(RH_KFE+6, RH_HFE+6) = F(iit::rbd::AZ,RH_KFE);
+ DATA(RH_HFE+6, RH_KFE+6) = DATA(RH_KFE+6, RH_HFE+6);
+ Fcol(RH_KFE) = frcTransf -> fr_RH_hipassembly_X_fr_RH_upperleg * Fcol(RH_KFE);
+ DATA(RH_KFE+6, RH_HAA+6) = F(iit::rbd::AZ,RH_KFE);
+ DATA(RH_HAA+6, RH_KFE+6) = DATA(RH_KFE+6, RH_HAA+6);
+ Fcol(RH_KFE) = frcTransf -> fr_trunk_X_fr_RH_hipassembly * Fcol(RH_KFE);
+
+ // Link RH_upperleg:
+ iit::rbd::transformInertia<Scalar>(RH_upperleg_Ic, frcTransf -> fr_RH_hipassembly_X_fr_RH_upperleg, Ic_spare);
+ RH_hipassembly_Ic += Ic_spare;
+
+ Fcol(RH_HFE) = RH_upperleg_Ic.col(iit::rbd::AZ);
+ DATA(RH_HFE+6, RH_HFE+6) = Fcol(RH_HFE)(iit::rbd::AZ);
+
+ Fcol(RH_HFE) = frcTransf -> fr_RH_hipassembly_X_fr_RH_upperleg * Fcol(RH_HFE);
+ DATA(RH_HFE+6, RH_HAA+6) = F(iit::rbd::AZ,RH_HFE);
+ DATA(RH_HAA+6, RH_HFE+6) = DATA(RH_HFE+6, RH_HAA+6);
+ Fcol(RH_HFE) = frcTransf -> fr_trunk_X_fr_RH_hipassembly * Fcol(RH_HFE);
+
+ // Link RH_hipassembly:
+ iit::rbd::transformInertia<Scalar>(RH_hipassembly_Ic, frcTransf -> fr_trunk_X_fr_RH_hipassembly, Ic_spare);
+ trunk_Ic += Ic_spare;
+
+ Fcol(RH_HAA) = RH_hipassembly_Ic.col(iit::rbd::AZ);
+ DATA(RH_HAA+6, RH_HAA+6) = Fcol(RH_HAA)(iit::rbd::AZ);
+
+ Fcol(RH_HAA) = frcTransf -> fr_trunk_X_fr_RH_hipassembly * Fcol(RH_HAA);
+
+ // Link LH_lowerleg:
+ iit::rbd::transformInertia<Scalar>(LH_lowerleg_Ic, frcTransf -> fr_LH_upperleg_X_fr_LH_lowerleg, Ic_spare);
+ LH_upperleg_Ic += Ic_spare;
+
+ Fcol(LH_KFE) = LH_lowerleg_Ic.col(iit::rbd::AZ);
+ DATA(LH_KFE+6, LH_KFE+6) = Fcol(LH_KFE)(iit::rbd::AZ);
+
+ Fcol(LH_KFE) = frcTransf -> fr_LH_upperleg_X_fr_LH_lowerleg * Fcol(LH_KFE);
+ DATA(LH_KFE+6, LH_HFE+6) = F(iit::rbd::AZ,LH_KFE);
+ DATA(LH_HFE+6, LH_KFE+6) = DATA(LH_KFE+6, LH_HFE+6);
+ Fcol(LH_KFE) = frcTransf -> fr_LH_hipassembly_X_fr_LH_upperleg * Fcol(LH_KFE);
+ DATA(LH_KFE+6, LH_HAA+6) = F(iit::rbd::AZ,LH_KFE);
+ DATA(LH_HAA+6, LH_KFE+6) = DATA(LH_KFE+6, LH_HAA+6);
+ Fcol(LH_KFE) = frcTransf -> fr_trunk_X_fr_LH_hipassembly * Fcol(LH_KFE);
+
+ // Link LH_upperleg:
+ iit::rbd::transformInertia<Scalar>(LH_upperleg_Ic, frcTransf -> fr_LH_hipassembly_X_fr_LH_upperleg, Ic_spare);
+ LH_hipassembly_Ic += Ic_spare;
+
+ Fcol(LH_HFE) = LH_upperleg_Ic.col(iit::rbd::AZ);
+ DATA(LH_HFE+6, LH_HFE+6) = Fcol(LH_HFE)(iit::rbd::AZ);
+
+ Fcol(LH_HFE) = frcTransf -> fr_LH_hipassembly_X_fr_LH_upperleg * Fcol(LH_HFE);
+ DATA(LH_HFE+6, LH_HAA+6) = F(iit::rbd::AZ,LH_HFE);
+ DATA(LH_HAA+6, LH_HFE+6) = DATA(LH_HFE+6, LH_HAA+6);
+ Fcol(LH_HFE) = frcTransf -> fr_trunk_X_fr_LH_hipassembly * Fcol(LH_HFE);
+
+ // Link LH_hipassembly:
+ iit::rbd::transformInertia<Scalar>(LH_hipassembly_Ic, frcTransf -> fr_trunk_X_fr_LH_hipassembly, Ic_spare);
+ trunk_Ic += Ic_spare;
+
+ Fcol(LH_HAA) = LH_hipassembly_Ic.col(iit::rbd::AZ);
+ DATA(LH_HAA+6, LH_HAA+6) = Fcol(LH_HAA)(iit::rbd::AZ);
+
+ Fcol(LH_HAA) = frcTransf -> fr_trunk_X_fr_LH_hipassembly * Fcol(LH_HAA);
+
+ // Link RF_lowerleg:
+ iit::rbd::transformInertia<Scalar>(RF_lowerleg_Ic, frcTransf -> fr_RF_upperleg_X_fr_RF_lowerleg, Ic_spare);
+ RF_upperleg_Ic += Ic_spare;
+
+ Fcol(RF_KFE) = RF_lowerleg_Ic.col(iit::rbd::AZ);
+ DATA(RF_KFE+6, RF_KFE+6) = Fcol(RF_KFE)(iit::rbd::AZ);
+
+ Fcol(RF_KFE) = frcTransf -> fr_RF_upperleg_X_fr_RF_lowerleg * Fcol(RF_KFE);
+ DATA(RF_KFE+6, RF_HFE+6) = F(iit::rbd::AZ,RF_KFE);
+ DATA(RF_HFE+6, RF_KFE+6) = DATA(RF_KFE+6, RF_HFE+6);
+ Fcol(RF_KFE) = frcTransf -> fr_RF_hipassembly_X_fr_RF_upperleg * Fcol(RF_KFE);
+ DATA(RF_KFE+6, RF_HAA+6) = F(iit::rbd::AZ,RF_KFE);
+ DATA(RF_HAA+6, RF_KFE+6) = DATA(RF_KFE+6, RF_HAA+6);
+ Fcol(RF_KFE) = frcTransf -> fr_trunk_X_fr_RF_hipassembly * Fcol(RF_KFE);
+
+ // Link RF_upperleg:
+ iit::rbd::transformInertia<Scalar>(RF_upperleg_Ic, frcTransf -> fr_RF_hipassembly_X_fr_RF_upperleg, Ic_spare);
+ RF_hipassembly_Ic += Ic_spare;
+
+ Fcol(RF_HFE) = RF_upperleg_Ic.col(iit::rbd::AZ);
+ DATA(RF_HFE+6, RF_HFE+6) = Fcol(RF_HFE)(iit::rbd::AZ);
+
+ Fcol(RF_HFE) = frcTransf -> fr_RF_hipassembly_X_fr_RF_upperleg * Fcol(RF_HFE);
+ DATA(RF_HFE+6, RF_HAA+6) = F(iit::rbd::AZ,RF_HFE);
+ DATA(RF_HAA+6, RF_HFE+6) = DATA(RF_HFE+6, RF_HAA+6);
+ Fcol(RF_HFE) = frcTransf -> fr_trunk_X_fr_RF_hipassembly * Fcol(RF_HFE);
+
+ // Link RF_hipassembly:
+ iit::rbd::transformInertia<Scalar>(RF_hipassembly_Ic, frcTransf -> fr_trunk_X_fr_RF_hipassembly, Ic_spare);
+ trunk_Ic += Ic_spare;
+
+ Fcol(RF_HAA) = RF_hipassembly_Ic.col(iit::rbd::AZ);
+ DATA(RF_HAA+6, RF_HAA+6) = Fcol(RF_HAA)(iit::rbd::AZ);
+
+ Fcol(RF_HAA) = frcTransf -> fr_trunk_X_fr_RF_hipassembly * Fcol(RF_HAA);
+
+ // Link LF_lowerleg:
+ iit::rbd::transformInertia<Scalar>(LF_lowerleg_Ic, frcTransf -> fr_LF_upperleg_X_fr_LF_lowerleg, Ic_spare);
+ LF_upperleg_Ic += Ic_spare;
+
+ Fcol(LF_KFE) = LF_lowerleg_Ic.col(iit::rbd::AZ);
+ DATA(LF_KFE+6, LF_KFE+6) = Fcol(LF_KFE)(iit::rbd::AZ);
+
+ Fcol(LF_KFE) = frcTransf -> fr_LF_upperleg_X_fr_LF_lowerleg * Fcol(LF_KFE);
+ DATA(LF_KFE+6, LF_HFE+6) = F(iit::rbd::AZ,LF_KFE);
+ DATA(LF_HFE+6, LF_KFE+6) = DATA(LF_KFE+6, LF_HFE+6);
+ Fcol(LF_KFE) = frcTransf -> fr_LF_hipassembly_X_fr_LF_upperleg * Fcol(LF_KFE);
+ DATA(LF_KFE+6, LF_HAA+6) = F(iit::rbd::AZ,LF_KFE);
+ DATA(LF_HAA+6, LF_KFE+6) = DATA(LF_KFE+6, LF_HAA+6);
+ Fcol(LF_KFE) = frcTransf -> fr_trunk_X_fr_LF_hipassembly * Fcol(LF_KFE);
+
+ // Link LF_upperleg:
+ iit::rbd::transformInertia<Scalar>(LF_upperleg_Ic, frcTransf -> fr_LF_hipassembly_X_fr_LF_upperleg, Ic_spare);
+ LF_hipassembly_Ic += Ic_spare;
+
+ Fcol(LF_HFE) = LF_upperleg_Ic.col(iit::rbd::AZ);
+ DATA(LF_HFE+6, LF_HFE+6) = Fcol(LF_HFE)(iit::rbd::AZ);
+
+ Fcol(LF_HFE) = frcTransf -> fr_LF_hipassembly_X_fr_LF_upperleg * Fcol(LF_HFE);
+ DATA(LF_HFE+6, LF_HAA+6) = F(iit::rbd::AZ,LF_HFE);
+ DATA(LF_HAA+6, LF_HFE+6) = DATA(LF_HFE+6, LF_HAA+6);
+ Fcol(LF_HFE) = frcTransf -> fr_trunk_X_fr_LF_hipassembly * Fcol(LF_HFE);
+
+ // Link LF_hipassembly:
+ iit::rbd::transformInertia<Scalar>(LF_hipassembly_Ic, frcTransf -> fr_trunk_X_fr_LF_hipassembly, Ic_spare);
+ trunk_Ic += Ic_spare;
+
+ Fcol(LF_HAA) = LF_hipassembly_Ic.col(iit::rbd::AZ);
+ DATA(LF_HAA+6, LF_HAA+6) = Fcol(LF_HAA)(iit::rbd::AZ);
+
+ Fcol(LF_HAA) = frcTransf -> fr_trunk_X_fr_LF_hipassembly * Fcol(LF_HAA);
+
+ // Copies the upper-right block into the lower-left block, after transposing
+ JSIM<TRAIT>:: template block<12, 6>(6,0) = (JSIM<TRAIT>:: template block<6, 12>(0,6)).transpose();
+ // The composite-inertia of the whole robot is the upper-left quadrant of the JSIM
+ JSIM<TRAIT>:: template block<6,6>(0,0) = trunk_Ic;
+ return *this;
+}
+
+#undef DATA
+#undef F
+
+template <typename TRAIT>
+void iit::TestHyQ::dyn::tpl::JSIM<TRAIT>::computeL() {
+ L = this -> template triangularView<Eigen::Lower>();
+ // Joint RH_KFE, index 11 :
+ L(11, 11) = std::sqrt(L(11, 11));
+ L(11, 10) = L(11, 10) / L(11, 11);
+ L(11, 9) = L(11, 9) / L(11, 11);
+ L(10, 10) = L(10, 10) - L(11, 10) * L(11, 10);
+ L(10, 9) = L(10, 9) - L(11, 10) * L(11, 9);
+ L(9, 9) = L(9, 9) - L(11, 9) * L(11, 9);
+
+ // Joint RH_HFE, index 10 :
+ L(10, 10) = std::sqrt(L(10, 10));
+ L(10, 9) = L(10, 9) / L(10, 10);
+ L(9, 9) = L(9, 9) - L(10, 9) * L(10, 9);
+
+ // Joint RH_HAA, index 9 :
+ L(9, 9) = std::sqrt(L(9, 9));
+
+ // Joint LH_KFE, index 8 :
+ L(8, 8) = std::sqrt(L(8, 8));
+ L(8, 7) = L(8, 7) / L(8, 8);
+ L(8, 6) = L(8, 6) / L(8, 8);
+ L(7, 7) = L(7, 7) - L(8, 7) * L(8, 7);
+ L(7, 6) = L(7, 6) - L(8, 7) * L(8, 6);
+ L(6, 6) = L(6, 6) - L(8, 6) * L(8, 6);
+
+ // Joint LH_HFE, index 7 :
+ L(7, 7) = std::sqrt(L(7, 7));
+ L(7, 6) = L(7, 6) / L(7, 7);
+ L(6, 6) = L(6, 6) - L(7, 6) * L(7, 6);
+
+ // Joint LH_HAA, index 6 :
+ L(6, 6) = std::sqrt(L(6, 6));
+
+ // Joint RF_KFE, index 5 :
+ L(5, 5) = std::sqrt(L(5, 5));
+ L(5, 4) = L(5, 4) / L(5, 5);
+ L(5, 3) = L(5, 3) / L(5, 5);
+ L(4, 4) = L(4, 4) - L(5, 4) * L(5, 4);
+ L(4, 3) = L(4, 3) - L(5, 4) * L(5, 3);
+ L(3, 3) = L(3, 3) - L(5, 3) * L(5, 3);
+
+ // Joint RF_HFE, index 4 :
+ L(4, 4) = std::sqrt(L(4, 4));
+ L(4, 3) = L(4, 3) / L(4, 4);
+ L(3, 3) = L(3, 3) - L(4, 3) * L(4, 3);
+
+ // Joint RF_HAA, index 3 :
+ L(3, 3) = std::sqrt(L(3, 3));
+
+ // Joint LF_KFE, index 2 :
+ L(2, 2) = std::sqrt(L(2, 2));
+ L(2, 1) = L(2, 1) / L(2, 2);
+ L(2, 0) = L(2, 0) / L(2, 2);
+ L(1, 1) = L(1, 1) - L(2, 1) * L(2, 1);
+ L(1, 0) = L(1, 0) - L(2, 1) * L(2, 0);
+ L(0, 0) = L(0, 0) - L(2, 0) * L(2, 0);
+
+ // Joint LF_HFE, index 1 :
+ L(1, 1) = std::sqrt(L(1, 1));
+ L(1, 0) = L(1, 0) / L(1, 1);
+ L(0, 0) = L(0, 0) - L(1, 0) * L(1, 0);
+
+ // Joint LF_HAA, index 0 :
+ L(0, 0) = std::sqrt(L(0, 0));
+
+}
+
+template <typename TRAIT>
+void iit::TestHyQ::dyn::tpl::JSIM<TRAIT>::computeInverse() {
+ computeLInverse();
+
+ inverse(0, 0) = + (Linv(0, 0) * Linv(0, 0));
+ inverse(1, 1) = + (Linv(1, 0) * Linv(1, 0)) + (Linv(1, 1) * Linv(1, 1));
+ inverse(1, 0) = + (Linv(1, 0) * Linv(0, 0));
+ inverse(0, 1) = inverse(1, 0);
+ inverse(2, 2) = + (Linv(2, 0) * Linv(2, 0)) + (Linv(2, 1) * Linv(2, 1)) + (Linv(2, 2) * Linv(2, 2));
+ inverse(2, 1) = + (Linv(2, 0) * Linv(1, 0)) + (Linv(2, 1) * Linv(1, 1));
+ inverse(1, 2) = inverse(2, 1);
+ inverse(2, 0) = + (Linv(2, 0) * Linv(0, 0));
+ inverse(0, 2) = inverse(2, 0);
+ inverse(3, 3) = + (Linv(3, 3) * Linv(3, 3));
+ inverse(4, 4) = + (Linv(4, 3) * Linv(4, 3)) + (Linv(4, 4) * Linv(4, 4));
+ inverse(4, 3) = + (Linv(4, 3) * Linv(3, 3));
+ inverse(3, 4) = inverse(4, 3);
+ inverse(5, 5) = + (Linv(5, 3) * Linv(5, 3)) + (Linv(5, 4) * Linv(5, 4)) + (Linv(5, 5) * Linv(5, 5));
+ inverse(5, 4) = + (Linv(5, 3) * Linv(4, 3)) + (Linv(5, 4) * Linv(4, 4));
+ inverse(4, 5) = inverse(5, 4);
+ inverse(5, 3) = + (Linv(5, 3) * Linv(3, 3));
+ inverse(3, 5) = inverse(5, 3);
+ inverse(6, 6) = + (Linv(6, 6) * Linv(6, 6));
+ inverse(7, 7) = + (Linv(7, 6) * Linv(7, 6)) + (Linv(7, 7) * Linv(7, 7));
+ inverse(7, 6) = + (Linv(7, 6) * Linv(6, 6));
+ inverse(6, 7) = inverse(7, 6);
+ inverse(8, 8) = + (Linv(8, 6) * Linv(8, 6)) + (Linv(8, 7) * Linv(8, 7)) + (Linv(8, 8) * Linv(8, 8));
+ inverse(8, 7) = + (Linv(8, 6) * Linv(7, 6)) + (Linv(8, 7) * Linv(7, 7));
+ inverse(7, 8) = inverse(8, 7);
+ inverse(8, 6) = + (Linv(8, 6) * Linv(6, 6));
+ inverse(6, 8) = inverse(8, 6);
+ inverse(9, 9) = + (Linv(9, 9) * Linv(9, 9));
+ inverse(10, 10) = + (Linv(10, 9) * Linv(10, 9)) + (Linv(10, 10) * Linv(10, 10));
+ inverse(10, 9) = + (Linv(10, 9) * Linv(9, 9));
+ inverse(9, 10) = inverse(10, 9);
+ inverse(11, 11) = + (Linv(11, 9) * Linv(11, 9)) + (Linv(11, 10) * Linv(11, 10)) + (Linv(11, 11) * Linv(11, 11));
+ inverse(11, 10) = + (Linv(11, 9) * Linv(10, 9)) + (Linv(11, 10) * Linv(10, 10));
+ inverse(10, 11) = inverse(11, 10);
+ inverse(11, 9) = + (Linv(11, 9) * Linv(9, 9));
+ inverse(9, 11) = inverse(11, 9);
+}
+
+template <typename TRAIT>
+void iit::TestHyQ::dyn::tpl::JSIM<TRAIT>::computeLInverse() {
+ //assumes L has been computed already
+ Linv(0, 0) = 1 / L(0, 0);
+ Linv(1, 1) = 1 / L(1, 1);
+ Linv(2, 2) = 1 / L(2, 2);
+ Linv(3, 3) = 1 / L(3, 3);
+ Linv(4, 4) = 1 / L(4, 4);
+ Linv(5, 5) = 1 / L(5, 5);
+ Linv(6, 6) = 1 / L(6, 6);
+ Linv(7, 7) = 1 / L(7, 7);
+ Linv(8, 8) = 1 / L(8, 8);
+ Linv(9, 9) = 1 / L(9, 9);
+ Linv(10, 10) = 1 / L(10, 10);
+ Linv(11, 11) = 1 / L(11, 11);
+ Linv(1, 0) = - Linv(0, 0) * ((Linv(1, 1) * L(1, 0)) + 0);
+ Linv(2, 1) = - Linv(1, 1) * ((Linv(2, 2) * L(2, 1)) + 0);
+ Linv(2, 0) = - Linv(0, 0) * ((Linv(2, 1) * L(1, 0)) + (Linv(2, 2) * L(2, 0)) + 0);
+ Linv(4, 3) = - Linv(3, 3) * ((Linv(4, 4) * L(4, 3)) + 0);
+ Linv(5, 4) = - Linv(4, 4) * ((Linv(5, 5) * L(5, 4)) + 0);
+ Linv(5, 3) = - Linv(3, 3) * ((Linv(5, 4) * L(4, 3)) + (Linv(5, 5) * L(5, 3)) + 0);
+ Linv(7, 6) = - Linv(6, 6) * ((Linv(7, 7) * L(7, 6)) + 0);
+ Linv(8, 7) = - Linv(7, 7) * ((Linv(8, 8) * L(8, 7)) + 0);
+ Linv(8, 6) = - Linv(6, 6) * ((Linv(8, 7) * L(7, 6)) + (Linv(8, 8) * L(8, 6)) + 0);
+ Linv(10, 9) = - Linv(9, 9) * ((Linv(10, 10) * L(10, 9)) + 0);
+ Linv(11, 10) = - Linv(10, 10) * ((Linv(11, 11) * L(11, 10)) + 0);
+ Linv(11, 9) = - Linv(9, 9) * ((Linv(11, 10) * L(10, 9)) + (Linv(11, 11) * L(11, 9)) + 0);
+}
+
diff --git a/ct_rbd/test/models/testhyq/generated/kinematics_parameters.h b/ct_rbd/test/models/testhyq/generated/kinematics_parameters.h
new file mode 100644
index 0000000..57ba55e
--- /dev/null
+++ b/ct_rbd/test/models/testhyq/generated/kinematics_parameters.h
@@ -0,0 +1,11 @@
+#ifndef _TESTHYQ_PARAMETERS_GETTERS_
+#define _TESTHYQ_PARAMETERS_GETTERS_
+
+namespace iit {
+namespace TestHyQ {
+
+
+
+}
+}
+#endif
diff --git a/ct_rbd/test/models/testhyq/generated/link_data_map.h b/ct_rbd/test/models/testhyq/generated/link_data_map.h
new file mode 100644
index 0000000..8223a5a
--- /dev/null
+++ b/ct_rbd/test/models/testhyq/generated/link_data_map.h
@@ -0,0 +1,136 @@
+#ifndef IIT_TESTHYQ_LINK_DATA_MAP_H_
+#define IIT_TESTHYQ_LINK_DATA_MAP_H_
+
+#include "declarations.h"
+
+namespace iit {
+namespace TestHyQ {
+
+/**
+ * A very simple container to associate a generic data item to each link
+ */
+template<typename T> class LinkDataMap {
+private:
+ T data[linksCount];
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ LinkDataMap() {};
+ LinkDataMap(const T& defaultValue);
+ LinkDataMap(const LinkDataMap& rhs);
+ LinkDataMap& operator=(const LinkDataMap& rhs);
+ LinkDataMap& operator=(const T& rhs);
+ T& operator[](LinkIdentifiers which);
+ const T& operator[](LinkIdentifiers which) const;
+private:
+ void copydata(const LinkDataMap& rhs);
+ void assigndata(const T& commonValue);
+};
+
+template<typename T> inline
+LinkDataMap<T>::LinkDataMap(const T& value) {
+ assigndata(value);
+}
+
+template<typename T> inline
+LinkDataMap<T>::LinkDataMap(const LinkDataMap& rhs)
+{
+ copydata(rhs);
+}
+
+template<typename T> inline
+LinkDataMap<T>& LinkDataMap<T>::operator=(const LinkDataMap& rhs)
+{
+ if(&rhs != this) {
+ copydata(rhs);
+ }
+ return *this;
+}
+
+template<typename T> inline
+LinkDataMap<T>& LinkDataMap<T>::operator=(const T& value)
+{
+ assigndata(value);
+ return *this;
+}
+
+template<typename T> inline
+T& LinkDataMap<T>::operator[](LinkIdentifiers l) {
+ return data[l];
+}
+
+template<typename T> inline
+const T& LinkDataMap<T>::operator[](LinkIdentifiers l) const {
+ return data[l];
+}
+
+template<typename T> inline
+void LinkDataMap<T>::copydata(const LinkDataMap& rhs) {
+ data[TRUNK] = rhs[TRUNK];
+ data[LF_HIPASSEMBLY] = rhs[LF_HIPASSEMBLY];
+ data[LF_UPPERLEG] = rhs[LF_UPPERLEG];
+ data[LF_LOWERLEG] = rhs[LF_LOWERLEG];
+ data[RF_HIPASSEMBLY] = rhs[RF_HIPASSEMBLY];
+ data[RF_UPPERLEG] = rhs[RF_UPPERLEG];
+ data[RF_LOWERLEG] = rhs[RF_LOWERLEG];
+ data[LH_HIPASSEMBLY] = rhs[LH_HIPASSEMBLY];
+ data[LH_UPPERLEG] = rhs[LH_UPPERLEG];
+ data[LH_LOWERLEG] = rhs[LH_LOWERLEG];
+ data[RH_HIPASSEMBLY] = rhs[RH_HIPASSEMBLY];
+ data[RH_UPPERLEG] = rhs[RH_UPPERLEG];
+ data[RH_LOWERLEG] = rhs[RH_LOWERLEG];
+}
+
+template<typename T> inline
+void LinkDataMap<T>::assigndata(const T& value) {
+ data[TRUNK] = value;
+ data[LF_HIPASSEMBLY] = value;
+ data[LF_UPPERLEG] = value;
+ data[LF_LOWERLEG] = value;
+ data[RF_HIPASSEMBLY] = value;
+ data[RF_UPPERLEG] = value;
+ data[RF_LOWERLEG] = value;
+ data[LH_HIPASSEMBLY] = value;
+ data[LH_UPPERLEG] = value;
+ data[LH_LOWERLEG] = value;
+ data[RH_HIPASSEMBLY] = value;
+ data[RH_UPPERLEG] = value;
+ data[RH_LOWERLEG] = value;
+}
+
+template<typename T> inline
+std::ostream& operator<<(std::ostream& out, const LinkDataMap<T>& map) {
+ out
+ << " trunk = "
+ << map[TRUNK]
+ << " LF_hipassembly = "
+ << map[LF_HIPASSEMBLY]
+ << " LF_upperleg = "
+ << map[LF_UPPERLEG]
+ << " LF_lowerleg = "
+ << map[LF_LOWERLEG]
+ << " RF_hipassembly = "
+ << map[RF_HIPASSEMBLY]
+ << " RF_upperleg = "
+ << map[RF_UPPERLEG]
+ << " RF_lowerleg = "
+ << map[RF_LOWERLEG]
+ << " LH_hipassembly = "
+ << map[LH_HIPASSEMBLY]
+ << " LH_upperleg = "
+ << map[LH_UPPERLEG]
+ << " LH_lowerleg = "
+ << map[LH_LOWERLEG]
+ << " RH_hipassembly = "
+ << map[RH_HIPASSEMBLY]
+ << " RH_upperleg = "
+ << map[RH_UPPERLEG]
+ << " RH_lowerleg = "
+ << map[RH_LOWERLEG]
+ ;
+ return out;
+}
+
+}
+}
+#endif
diff --git a/ct_rbd/test/models/testhyq/generated/miscellaneous.cpp b/ct_rbd/test/models/testhyq/generated/miscellaneous.cpp
new file mode 100644
index 0000000..334635b
--- /dev/null
+++ b/ct_rbd/test/models/testhyq/generated/miscellaneous.cpp
@@ -0,0 +1,95 @@
+#include <iit/rbd/utils.h>
+#include "miscellaneous.h"
+
+using namespace iit::TestHyQ;
+using namespace iit::TestHyQ::dyn;
+
+iit::rbd::Vector3d iit::TestHyQ::getWholeBodyCOM(
+ const InertiaProperties& inertiaProps,
+ const HomogeneousTransforms& ht)
+{
+ iit::rbd::Vector3d tmpSum(iit::rbd::Vector3d::Zero());
+
+ tmpSum += inertiaProps.getCOM_trunk() * inertiaProps.getMass_trunk();
+
+ HomogeneousTransforms::MatrixType tmpX(HomogeneousTransforms::MatrixType::Identity());
+ HomogeneousTransforms::MatrixType base_X_LF_HAA_chain;
+ HomogeneousTransforms::MatrixType base_X_RF_HAA_chain;
+ HomogeneousTransforms::MatrixType base_X_LH_HAA_chain;
+ HomogeneousTransforms::MatrixType base_X_RH_HAA_chain;
+
+
+ base_X_LF_HAA_chain = tmpX * ht.fr_trunk_X_fr_LF_hipassembly;
+ tmpSum += inertiaProps.getMass_LF_hipassembly() *
+ ( iit::rbd::Utils::transform(base_X_LF_HAA_chain, inertiaProps.getCOM_LF_hipassembly()));
+
+ base_X_LF_HAA_chain = base_X_LF_HAA_chain * ht.fr_LF_hipassembly_X_fr_LF_upperleg;
+ tmpSum += inertiaProps.getMass_LF_upperleg() *
+ ( iit::rbd::Utils::transform(base_X_LF_HAA_chain, inertiaProps.getCOM_LF_upperleg()));
+
+ base_X_LF_HAA_chain = base_X_LF_HAA_chain * ht.fr_LF_upperleg_X_fr_LF_lowerleg;
+ tmpSum += inertiaProps.getMass_LF_lowerleg() *
+ ( iit::rbd::Utils::transform(base_X_LF_HAA_chain, inertiaProps.getCOM_LF_lowerleg()));
+
+ base_X_RF_HAA_chain = tmpX * ht.fr_trunk_X_fr_RF_hipassembly;
+ tmpSum += inertiaProps.getMass_RF_hipassembly() *
+ ( iit::rbd::Utils::transform(base_X_RF_HAA_chain, inertiaProps.getCOM_RF_hipassembly()));
+
+ base_X_RF_HAA_chain = base_X_RF_HAA_chain * ht.fr_RF_hipassembly_X_fr_RF_upperleg;
+ tmpSum += inertiaProps.getMass_RF_upperleg() *
+ ( iit::rbd::Utils::transform(base_X_RF_HAA_chain, inertiaProps.getCOM_RF_upperleg()));
+
+ base_X_RF_HAA_chain = base_X_RF_HAA_chain * ht.fr_RF_upperleg_X_fr_RF_lowerleg;
+ tmpSum += inertiaProps.getMass_RF_lowerleg() *
+ ( iit::rbd::Utils::transform(base_X_RF_HAA_chain, inertiaProps.getCOM_RF_lowerleg()));
+
+ base_X_LH_HAA_chain = tmpX * ht.fr_trunk_X_fr_LH_hipassembly;
+ tmpSum += inertiaProps.getMass_LH_hipassembly() *
+ ( iit::rbd::Utils::transform(base_X_LH_HAA_chain, inertiaProps.getCOM_LH_hipassembly()));
+
+ base_X_LH_HAA_chain = base_X_LH_HAA_chain * ht.fr_LH_hipassembly_X_fr_LH_upperleg;
+ tmpSum += inertiaProps.getMass_LH_upperleg() *
+ ( iit::rbd::Utils::transform(base_X_LH_HAA_chain, inertiaProps.getCOM_LH_upperleg()));
+
+ base_X_LH_HAA_chain = base_X_LH_HAA_chain * ht.fr_LH_upperleg_X_fr_LH_lowerleg;
+ tmpSum += inertiaProps.getMass_LH_lowerleg() *
+ ( iit::rbd::Utils::transform(base_X_LH_HAA_chain, inertiaProps.getCOM_LH_lowerleg()));
+
+ base_X_RH_HAA_chain = tmpX * ht.fr_trunk_X_fr_RH_hipassembly;
+ tmpSum += inertiaProps.getMass_RH_hipassembly() *
+ ( iit::rbd::Utils::transform(base_X_RH_HAA_chain, inertiaProps.getCOM_RH_hipassembly()));
+
+ base_X_RH_HAA_chain = base_X_RH_HAA_chain * ht.fr_RH_hipassembly_X_fr_RH_upperleg;
+ tmpSum += inertiaProps.getMass_RH_upperleg() *
+ ( iit::rbd::Utils::transform(base_X_RH_HAA_chain, inertiaProps.getCOM_RH_upperleg()));
+
+ base_X_RH_HAA_chain = base_X_RH_HAA_chain * ht.fr_RH_upperleg_X_fr_RH_lowerleg;
+ tmpSum += inertiaProps.getMass_RH_lowerleg() *
+ ( iit::rbd::Utils::transform(base_X_RH_HAA_chain, inertiaProps.getCOM_RH_lowerleg()));
+
+
+ return tmpSum / inertiaProps.getTotalMass();
+}
+
+iit::rbd::Vector3d iit::TestHyQ::getWholeBodyCOM(
+ const InertiaProperties& inertiaProps,
+ const JointState& q,
+ HomogeneousTransforms& ht)
+{
+ // First updates the coordinate transforms that will be used by the routine
+ ht.fr_trunk_X_fr_LF_hipassembly(q);
+ ht.fr_trunk_X_fr_RF_hipassembly(q);
+ ht.fr_trunk_X_fr_LH_hipassembly(q);
+ ht.fr_trunk_X_fr_RH_hipassembly(q);
+ ht.fr_LF_hipassembly_X_fr_LF_upperleg(q);
+ ht.fr_LF_upperleg_X_fr_LF_lowerleg(q);
+ ht.fr_RF_hipassembly_X_fr_RF_upperleg(q);
+ ht.fr_RF_upperleg_X_fr_RF_lowerleg(q);
+ ht.fr_LH_hipassembly_X_fr_LH_upperleg(q);
+ ht.fr_LH_upperleg_X_fr_LH_lowerleg(q);
+ ht.fr_RH_hipassembly_X_fr_RH_upperleg(q);
+ ht.fr_RH_upperleg_X_fr_RH_lowerleg(q);
+
+ // The actual calculus
+ return getWholeBodyCOM(inertiaProps, ht);
+}
diff --git a/ct_rbd/test/models/testhyq/generated/miscellaneous.h b/ct_rbd/test/models/testhyq/generated/miscellaneous.h
new file mode 100644
index 0000000..ed27002
--- /dev/null
+++ b/ct_rbd/test/models/testhyq/generated/miscellaneous.h
@@ -0,0 +1,45 @@
+#ifndef IIT_ROBCOGEN__TESTHYQ_MISCELLANEOUS_H_
+#define IIT_ROBCOGEN__TESTHYQ_MISCELLANEOUS_H_
+
+#include "inertia_properties.h"
+#include "transforms.h"
+
+namespace iit {
+namespace TestHyQ {
+
+/** \name Center of mass calculation
+ * Computes the Center Of Mass (COM) position of the whole robot, in
+ * base coordinates.
+ *
+ * Common parameters are the inertia properties of the robot and the set
+ * of homogeneous coordinate transforms. If a joint status variable is
+ * also passed, then the transforms are updated accordingly; otherwise,
+ * they are not modified before being used.
+ */
+///@{
+/**
+ * \param inertia the inertia properties of the links of the robot
+ * \param transforms the homogeneous coordinate transforms of the robot
+ * \return the position of the Center Of Mass of the whole robot, expressed
+ * in base coordinates
+ */
+iit::rbd::Vector3d getWholeBodyCOM(
+ const dyn::InertiaProperties& inertia,
+ const HomogeneousTransforms& transforms);
+/**
+ * \param inertia the inertia properties of the links of the robot
+ * \param q the joint status vector describing the configuration of the robot
+ * \param transforms the homogeneous coordinate transforms of the robot
+ * \return the position of the Center Of Mass of the whole robot, expressed
+ * in base coordinates
+ */
+iit::rbd::Vector3d getWholeBodyCOM(
+ const dyn::InertiaProperties& inertia,
+ const JointState& q,
+ HomogeneousTransforms& transforms);
+///@}
+
+}
+}
+
+#endif
diff --git a/ct_rbd/test/models/testhyq/generated/traits.h b/ct_rbd/test/models/testhyq/generated/traits.h
new file mode 100644
index 0000000..6347b33
--- /dev/null
+++ b/ct_rbd/test/models/testhyq/generated/traits.h
@@ -0,0 +1,66 @@
+#ifndef IIT_ROBOGEN__TESTHYQ_TRAITS_H_
+#define IIT_ROBOGEN__TESTHYQ_TRAITS_H_
+
+#include "declarations.h"
+#include "transforms.h"
+#include "inverse_dynamics.h"
+#include "forward_dynamics.h"
+#include "jsim.h"
+#include "inertia_properties.h"
+#include "jacobians.h"
+#include <iit/rbd/traits/TraitSelector.h>
+
+
+namespace iit {
+namespace TestHyQ {
+
+namespace tpl {
+
+template <typename SCALAR>
+struct Traits {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef SCALAR S;
+
+ typedef typename iit::rbd::tpl::TraitSelector<SCALAR>::Trait Trait;
+
+ typedef typename iit::TestHyQ::tpl::JointState<SCALAR> JointState;
+
+ typedef typename iit::TestHyQ::JointIdentifiers JointID;
+ typedef typename iit::TestHyQ::LinkIdentifiers LinkID;
+
+ typedef typename iit::TestHyQ::tpl::HomogeneousTransforms<Trait> HomogeneousTransforms;
+ typedef typename iit::TestHyQ::tpl::MotionTransforms<Trait> MotionTransforms;
+ typedef typename iit::TestHyQ::tpl::ForceTransforms<Trait> ForceTransforms;
+ typedef typename iit::TestHyQ::tpl::Jacobians<Trait> Jacobians;
+
+ typedef typename iit::TestHyQ::dyn::tpl::InertiaProperties<Trait> InertiaProperties;
+ typedef typename iit::TestHyQ::dyn::tpl::ForwardDynamics<Trait> FwdDynEngine;
+ typedef typename iit::TestHyQ::dyn::tpl::InverseDynamics<Trait> InvDynEngine;
+ typedef typename iit::TestHyQ::dyn::tpl::JSIM<Trait> JSIM;
+
+ static const int joints_count = iit::TestHyQ::jointsCount;
+ static const int links_count = iit::TestHyQ::linksCount;
+ static const bool floating_base = true;
+
+ static inline const JointID* orderedJointIDs();
+ static inline const LinkID* orderedLinkIDs();
+};
+
+template <typename SCALAR>
+inline const typename Traits<SCALAR>::JointID* Traits<SCALAR>::orderedJointIDs() {
+ return iit::TestHyQ::orderedJointIDs;
+}
+template <typename SCALAR>
+inline const typename Traits<SCALAR>::LinkID* Traits<SCALAR>::orderedLinkIDs() {
+ return iit::TestHyQ::orderedLinkIDs;
+}
+
+}
+
+typedef tpl::Traits<double> Traits;
+
+}
+}
+
+#endif
diff --git a/ct_rbd/test/models/testhyq/generated/transforms.h b/ct_rbd/test/models/testhyq/generated/transforms.h
new file mode 100644
index 0000000..7bd4977
--- /dev/null
+++ b/ct_rbd/test/models/testhyq/generated/transforms.h
@@ -0,0 +1,2391 @@
+#ifndef TESTHYQ_TRANSFORMS_H_
+#define TESTHYQ_TRANSFORMS_H_
+
+#include <Eigen/Dense>
+#include <iit/rbd/TransformsBase.h>
+#include "declarations.h"
+#include <iit/rbd/traits/DoubleTrait.h>
+#include "kinematics_parameters.h"
+
+namespace iit {
+namespace TestHyQ {
+
+template<typename SCALAR, class M>
+class TransformMotion : public iit::rbd::SpatialTransformBase<tpl::JointState<SCALAR>, M> {
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+template<typename SCALAR, class M>
+class TransformForce : public iit::rbd::SpatialTransformBase<tpl::JointState<SCALAR>, M> {
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+template<typename SCALAR, class M>
+class TransformHomogeneous : public iit::rbd::HomogeneousTransformBase<tpl::JointState<SCALAR>, M> {
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+namespace tpl {
+
+
+/**
+ * The class for the 6-by-6 coordinates transformation matrices for
+ * spatial motion vectors.
+ */
+template <typename TRAIT>
+class MotionTransforms {
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef typename TRAIT::Scalar Scalar;
+
+ typedef JointState<Scalar> JState;
+ class Dummy {};
+ typedef typename TransformMotion<Scalar, Dummy>::MatrixType MatrixType;
+public:
+ class Type_fr_trunk_X_fr_LF_hipassembly : public TransformMotion<Scalar, Type_fr_trunk_X_fr_LF_hipassembly>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LF_hipassembly();
+ const Type_fr_trunk_X_fr_LF_hipassembly& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RF_hipassembly : public TransformMotion<Scalar, Type_fr_trunk_X_fr_RF_hipassembly>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RF_hipassembly();
+ const Type_fr_trunk_X_fr_RF_hipassembly& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LH_hipassembly : public TransformMotion<Scalar, Type_fr_trunk_X_fr_LH_hipassembly>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LH_hipassembly();
+ const Type_fr_trunk_X_fr_LH_hipassembly& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RH_hipassembly : public TransformMotion<Scalar, Type_fr_trunk_X_fr_RH_hipassembly>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RH_hipassembly();
+ const Type_fr_trunk_X_fr_RH_hipassembly& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LF_upperleg : public TransformMotion<Scalar, Type_fr_trunk_X_fr_LF_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LF_upperleg();
+ const Type_fr_trunk_X_fr_LF_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RF_upperleg : public TransformMotion<Scalar, Type_fr_trunk_X_fr_RF_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RF_upperleg();
+ const Type_fr_trunk_X_fr_RF_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LH_upperleg : public TransformMotion<Scalar, Type_fr_trunk_X_fr_LH_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LH_upperleg();
+ const Type_fr_trunk_X_fr_LH_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RH_upperleg : public TransformMotion<Scalar, Type_fr_trunk_X_fr_RH_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RH_upperleg();
+ const Type_fr_trunk_X_fr_RH_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LF_lowerleg : public TransformMotion<Scalar, Type_fr_trunk_X_fr_LF_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LF_lowerleg();
+ const Type_fr_trunk_X_fr_LF_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RF_lowerleg : public TransformMotion<Scalar, Type_fr_trunk_X_fr_RF_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RF_lowerleg();
+ const Type_fr_trunk_X_fr_RF_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LH_lowerleg : public TransformMotion<Scalar, Type_fr_trunk_X_fr_LH_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LH_lowerleg();
+ const Type_fr_trunk_X_fr_LH_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RH_lowerleg : public TransformMotion<Scalar, Type_fr_trunk_X_fr_RH_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RH_lowerleg();
+ const Type_fr_trunk_X_fr_RH_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LF_hipassembly_X_fr_trunk : public TransformMotion<Scalar, Type_fr_LF_hipassembly_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LF_hipassembly_X_fr_trunk();
+ const Type_fr_LF_hipassembly_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RF_hipassembly_X_fr_trunk : public TransformMotion<Scalar, Type_fr_RF_hipassembly_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RF_hipassembly_X_fr_trunk();
+ const Type_fr_RF_hipassembly_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LH_hipassembly_X_fr_trunk : public TransformMotion<Scalar, Type_fr_LH_hipassembly_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LH_hipassembly_X_fr_trunk();
+ const Type_fr_LH_hipassembly_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RH_hipassembly_X_fr_trunk : public TransformMotion<Scalar, Type_fr_RH_hipassembly_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RH_hipassembly_X_fr_trunk();
+ const Type_fr_RH_hipassembly_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LF_upperleg_X_fr_trunk : public TransformMotion<Scalar, Type_fr_LF_upperleg_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LF_upperleg_X_fr_trunk();
+ const Type_fr_LF_upperleg_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RF_upperleg_X_fr_trunk : public TransformMotion<Scalar, Type_fr_RF_upperleg_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RF_upperleg_X_fr_trunk();
+ const Type_fr_RF_upperleg_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LH_upperleg_X_fr_trunk : public TransformMotion<Scalar, Type_fr_LH_upperleg_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LH_upperleg_X_fr_trunk();
+ const Type_fr_LH_upperleg_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RH_upperleg_X_fr_trunk : public TransformMotion<Scalar, Type_fr_RH_upperleg_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RH_upperleg_X_fr_trunk();
+ const Type_fr_RH_upperleg_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LF_lowerleg_X_fr_trunk : public TransformMotion<Scalar, Type_fr_LF_lowerleg_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LF_lowerleg_X_fr_trunk();
+ const Type_fr_LF_lowerleg_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RF_lowerleg_X_fr_trunk : public TransformMotion<Scalar, Type_fr_RF_lowerleg_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RF_lowerleg_X_fr_trunk();
+ const Type_fr_RF_lowerleg_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LH_lowerleg_X_fr_trunk : public TransformMotion<Scalar, Type_fr_LH_lowerleg_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LH_lowerleg_X_fr_trunk();
+ const Type_fr_LH_lowerleg_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RH_lowerleg_X_fr_trunk : public TransformMotion<Scalar, Type_fr_RH_lowerleg_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RH_lowerleg_X_fr_trunk();
+ const Type_fr_RH_lowerleg_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_LF_hipassemblyCOM : public TransformMotion<Scalar, Type_fr_trunk_X_LF_hipassemblyCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_LF_hipassemblyCOM();
+ const Type_fr_trunk_X_LF_hipassemblyCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_RF_hipassemblyCOM : public TransformMotion<Scalar, Type_fr_trunk_X_RF_hipassemblyCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_RF_hipassemblyCOM();
+ const Type_fr_trunk_X_RF_hipassemblyCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_LH_hipassemblyCOM : public TransformMotion<Scalar, Type_fr_trunk_X_LH_hipassemblyCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_LH_hipassemblyCOM();
+ const Type_fr_trunk_X_LH_hipassemblyCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_RH_hipassemblyCOM : public TransformMotion<Scalar, Type_fr_trunk_X_RH_hipassemblyCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_RH_hipassemblyCOM();
+ const Type_fr_trunk_X_RH_hipassemblyCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_LF_upperlegCOM : public TransformMotion<Scalar, Type_fr_trunk_X_LF_upperlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_LF_upperlegCOM();
+ const Type_fr_trunk_X_LF_upperlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_RF_upperlegCOM : public TransformMotion<Scalar, Type_fr_trunk_X_RF_upperlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_RF_upperlegCOM();
+ const Type_fr_trunk_X_RF_upperlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_LH_upperlegCOM : public TransformMotion<Scalar, Type_fr_trunk_X_LH_upperlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_LH_upperlegCOM();
+ const Type_fr_trunk_X_LH_upperlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_RH_upperlegCOM : public TransformMotion<Scalar, Type_fr_trunk_X_RH_upperlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_RH_upperlegCOM();
+ const Type_fr_trunk_X_RH_upperlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_LF_lowerlegCOM : public TransformMotion<Scalar, Type_fr_trunk_X_LF_lowerlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_LF_lowerlegCOM();
+ const Type_fr_trunk_X_LF_lowerlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_RF_lowerlegCOM : public TransformMotion<Scalar, Type_fr_trunk_X_RF_lowerlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_RF_lowerlegCOM();
+ const Type_fr_trunk_X_RF_lowerlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_LH_lowerlegCOM : public TransformMotion<Scalar, Type_fr_trunk_X_LH_lowerlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_LH_lowerlegCOM();
+ const Type_fr_trunk_X_LH_lowerlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_RH_lowerlegCOM : public TransformMotion<Scalar, Type_fr_trunk_X_RH_lowerlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_RH_lowerlegCOM();
+ const Type_fr_trunk_X_RH_lowerlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LF_foot_X_fr_LF_lowerleg : public TransformMotion<Scalar, Type_fr_LF_foot_X_fr_LF_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LF_foot_X_fr_LF_lowerleg();
+ const Type_fr_LF_foot_X_fr_LF_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RF_foot_X_fr_RF_lowerleg : public TransformMotion<Scalar, Type_fr_RF_foot_X_fr_RF_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RF_foot_X_fr_RF_lowerleg();
+ const Type_fr_RF_foot_X_fr_RF_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LH_foot_X_fr_LH_lowerleg : public TransformMotion<Scalar, Type_fr_LH_foot_X_fr_LH_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LH_foot_X_fr_LH_lowerleg();
+ const Type_fr_LH_foot_X_fr_LH_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RH_foot_X_fr_RH_lowerleg : public TransformMotion<Scalar, Type_fr_RH_foot_X_fr_RH_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RH_foot_X_fr_RH_lowerleg();
+ const Type_fr_RH_foot_X_fr_RH_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LF_foot : public TransformMotion<Scalar, Type_fr_trunk_X_fr_LF_foot>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LF_foot();
+ const Type_fr_trunk_X_fr_LF_foot& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RF_foot : public TransformMotion<Scalar, Type_fr_trunk_X_fr_RF_foot>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RF_foot();
+ const Type_fr_trunk_X_fr_RF_foot& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LH_foot : public TransformMotion<Scalar, Type_fr_trunk_X_fr_LH_foot>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LH_foot();
+ const Type_fr_trunk_X_fr_LH_foot& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RH_foot : public TransformMotion<Scalar, Type_fr_trunk_X_fr_RH_foot>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RH_foot();
+ const Type_fr_trunk_X_fr_RH_foot& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LF_foot_X_fr_trunk : public TransformMotion<Scalar, Type_fr_LF_foot_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LF_foot_X_fr_trunk();
+ const Type_fr_LF_foot_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RF_foot_X_fr_trunk : public TransformMotion<Scalar, Type_fr_RF_foot_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RF_foot_X_fr_trunk();
+ const Type_fr_RF_foot_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LH_foot_X_fr_trunk : public TransformMotion<Scalar, Type_fr_LH_foot_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LH_foot_X_fr_trunk();
+ const Type_fr_LH_foot_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RH_foot_X_fr_trunk : public TransformMotion<Scalar, Type_fr_RH_foot_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RH_foot_X_fr_trunk();
+ const Type_fr_RH_foot_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LF_HAA : public TransformMotion<Scalar, Type_fr_trunk_X_fr_LF_HAA>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LF_HAA();
+ const Type_fr_trunk_X_fr_LF_HAA& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RF_HAA : public TransformMotion<Scalar, Type_fr_trunk_X_fr_RF_HAA>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RF_HAA();
+ const Type_fr_trunk_X_fr_RF_HAA& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LH_HAA : public TransformMotion<Scalar, Type_fr_trunk_X_fr_LH_HAA>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LH_HAA();
+ const Type_fr_trunk_X_fr_LH_HAA& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RH_HAA : public TransformMotion<Scalar, Type_fr_trunk_X_fr_RH_HAA>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RH_HAA();
+ const Type_fr_trunk_X_fr_RH_HAA& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LF_HFE : public TransformMotion<Scalar, Type_fr_trunk_X_fr_LF_HFE>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LF_HFE();
+ const Type_fr_trunk_X_fr_LF_HFE& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RF_HFE : public TransformMotion<Scalar, Type_fr_trunk_X_fr_RF_HFE>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RF_HFE();
+ const Type_fr_trunk_X_fr_RF_HFE& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LH_HFE : public TransformMotion<Scalar, Type_fr_trunk_X_fr_LH_HFE>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LH_HFE();
+ const Type_fr_trunk_X_fr_LH_HFE& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RH_HFE : public TransformMotion<Scalar, Type_fr_trunk_X_fr_RH_HFE>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RH_HFE();
+ const Type_fr_trunk_X_fr_RH_HFE& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LF_KFE : public TransformMotion<Scalar, Type_fr_trunk_X_fr_LF_KFE>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LF_KFE();
+ const Type_fr_trunk_X_fr_LF_KFE& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RF_KFE : public TransformMotion<Scalar, Type_fr_trunk_X_fr_RF_KFE>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RF_KFE();
+ const Type_fr_trunk_X_fr_RF_KFE& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LH_KFE : public TransformMotion<Scalar, Type_fr_trunk_X_fr_LH_KFE>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LH_KFE();
+ const Type_fr_trunk_X_fr_LH_KFE& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RH_KFE : public TransformMotion<Scalar, Type_fr_trunk_X_fr_RH_KFE>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RH_KFE();
+ const Type_fr_trunk_X_fr_RH_KFE& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LF_upperleg_X_fr_LF_hipassembly : public TransformMotion<Scalar, Type_fr_LF_upperleg_X_fr_LF_hipassembly>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LF_upperleg_X_fr_LF_hipassembly();
+ const Type_fr_LF_upperleg_X_fr_LF_hipassembly& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LF_hipassembly_X_fr_LF_upperleg : public TransformMotion<Scalar, Type_fr_LF_hipassembly_X_fr_LF_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LF_hipassembly_X_fr_LF_upperleg();
+ const Type_fr_LF_hipassembly_X_fr_LF_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LF_lowerleg_X_fr_LF_upperleg : public TransformMotion<Scalar, Type_fr_LF_lowerleg_X_fr_LF_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LF_lowerleg_X_fr_LF_upperleg();
+ const Type_fr_LF_lowerleg_X_fr_LF_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LF_upperleg_X_fr_LF_lowerleg : public TransformMotion<Scalar, Type_fr_LF_upperleg_X_fr_LF_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LF_upperleg_X_fr_LF_lowerleg();
+ const Type_fr_LF_upperleg_X_fr_LF_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RF_upperleg_X_fr_RF_hipassembly : public TransformMotion<Scalar, Type_fr_RF_upperleg_X_fr_RF_hipassembly>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RF_upperleg_X_fr_RF_hipassembly();
+ const Type_fr_RF_upperleg_X_fr_RF_hipassembly& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RF_hipassembly_X_fr_RF_upperleg : public TransformMotion<Scalar, Type_fr_RF_hipassembly_X_fr_RF_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RF_hipassembly_X_fr_RF_upperleg();
+ const Type_fr_RF_hipassembly_X_fr_RF_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RF_lowerleg_X_fr_RF_upperleg : public TransformMotion<Scalar, Type_fr_RF_lowerleg_X_fr_RF_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RF_lowerleg_X_fr_RF_upperleg();
+ const Type_fr_RF_lowerleg_X_fr_RF_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RF_upperleg_X_fr_RF_lowerleg : public TransformMotion<Scalar, Type_fr_RF_upperleg_X_fr_RF_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RF_upperleg_X_fr_RF_lowerleg();
+ const Type_fr_RF_upperleg_X_fr_RF_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LH_upperleg_X_fr_LH_hipassembly : public TransformMotion<Scalar, Type_fr_LH_upperleg_X_fr_LH_hipassembly>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LH_upperleg_X_fr_LH_hipassembly();
+ const Type_fr_LH_upperleg_X_fr_LH_hipassembly& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LH_hipassembly_X_fr_LH_upperleg : public TransformMotion<Scalar, Type_fr_LH_hipassembly_X_fr_LH_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LH_hipassembly_X_fr_LH_upperleg();
+ const Type_fr_LH_hipassembly_X_fr_LH_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LH_lowerleg_X_fr_LH_upperleg : public TransformMotion<Scalar, Type_fr_LH_lowerleg_X_fr_LH_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LH_lowerleg_X_fr_LH_upperleg();
+ const Type_fr_LH_lowerleg_X_fr_LH_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LH_upperleg_X_fr_LH_lowerleg : public TransformMotion<Scalar, Type_fr_LH_upperleg_X_fr_LH_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LH_upperleg_X_fr_LH_lowerleg();
+ const Type_fr_LH_upperleg_X_fr_LH_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RH_upperleg_X_fr_RH_hipassembly : public TransformMotion<Scalar, Type_fr_RH_upperleg_X_fr_RH_hipassembly>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RH_upperleg_X_fr_RH_hipassembly();
+ const Type_fr_RH_upperleg_X_fr_RH_hipassembly& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RH_hipassembly_X_fr_RH_upperleg : public TransformMotion<Scalar, Type_fr_RH_hipassembly_X_fr_RH_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RH_hipassembly_X_fr_RH_upperleg();
+ const Type_fr_RH_hipassembly_X_fr_RH_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RH_lowerleg_X_fr_RH_upperleg : public TransformMotion<Scalar, Type_fr_RH_lowerleg_X_fr_RH_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RH_lowerleg_X_fr_RH_upperleg();
+ const Type_fr_RH_lowerleg_X_fr_RH_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RH_upperleg_X_fr_RH_lowerleg : public TransformMotion<Scalar, Type_fr_RH_upperleg_X_fr_RH_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RH_upperleg_X_fr_RH_lowerleg();
+ const Type_fr_RH_upperleg_X_fr_RH_lowerleg& update(const JState&);
+ protected:
+ };
+
+public:
+ MotionTransforms();
+ void updateParameters();
+ Type_fr_trunk_X_fr_LF_hipassembly fr_trunk_X_fr_LF_hipassembly;
+ Type_fr_trunk_X_fr_RF_hipassembly fr_trunk_X_fr_RF_hipassembly;
+ Type_fr_trunk_X_fr_LH_hipassembly fr_trunk_X_fr_LH_hipassembly;
+ Type_fr_trunk_X_fr_RH_hipassembly fr_trunk_X_fr_RH_hipassembly;
+ Type_fr_trunk_X_fr_LF_upperleg fr_trunk_X_fr_LF_upperleg;
+ Type_fr_trunk_X_fr_RF_upperleg fr_trunk_X_fr_RF_upperleg;
+ Type_fr_trunk_X_fr_LH_upperleg fr_trunk_X_fr_LH_upperleg;
+ Type_fr_trunk_X_fr_RH_upperleg fr_trunk_X_fr_RH_upperleg;
+ Type_fr_trunk_X_fr_LF_lowerleg fr_trunk_X_fr_LF_lowerleg;
+ Type_fr_trunk_X_fr_RF_lowerleg fr_trunk_X_fr_RF_lowerleg;
+ Type_fr_trunk_X_fr_LH_lowerleg fr_trunk_X_fr_LH_lowerleg;
+ Type_fr_trunk_X_fr_RH_lowerleg fr_trunk_X_fr_RH_lowerleg;
+ Type_fr_LF_hipassembly_X_fr_trunk fr_LF_hipassembly_X_fr_trunk;
+ Type_fr_RF_hipassembly_X_fr_trunk fr_RF_hipassembly_X_fr_trunk;
+ Type_fr_LH_hipassembly_X_fr_trunk fr_LH_hipassembly_X_fr_trunk;
+ Type_fr_RH_hipassembly_X_fr_trunk fr_RH_hipassembly_X_fr_trunk;
+ Type_fr_LF_upperleg_X_fr_trunk fr_LF_upperleg_X_fr_trunk;
+ Type_fr_RF_upperleg_X_fr_trunk fr_RF_upperleg_X_fr_trunk;
+ Type_fr_LH_upperleg_X_fr_trunk fr_LH_upperleg_X_fr_trunk;
+ Type_fr_RH_upperleg_X_fr_trunk fr_RH_upperleg_X_fr_trunk;
+ Type_fr_LF_lowerleg_X_fr_trunk fr_LF_lowerleg_X_fr_trunk;
+ Type_fr_RF_lowerleg_X_fr_trunk fr_RF_lowerleg_X_fr_trunk;
+ Type_fr_LH_lowerleg_X_fr_trunk fr_LH_lowerleg_X_fr_trunk;
+ Type_fr_RH_lowerleg_X_fr_trunk fr_RH_lowerleg_X_fr_trunk;
+ Type_fr_trunk_X_LF_hipassemblyCOM fr_trunk_X_LF_hipassemblyCOM;
+ Type_fr_trunk_X_RF_hipassemblyCOM fr_trunk_X_RF_hipassemblyCOM;
+ Type_fr_trunk_X_LH_hipassemblyCOM fr_trunk_X_LH_hipassemblyCOM;
+ Type_fr_trunk_X_RH_hipassemblyCOM fr_trunk_X_RH_hipassemblyCOM;
+ Type_fr_trunk_X_LF_upperlegCOM fr_trunk_X_LF_upperlegCOM;
+ Type_fr_trunk_X_RF_upperlegCOM fr_trunk_X_RF_upperlegCOM;
+ Type_fr_trunk_X_LH_upperlegCOM fr_trunk_X_LH_upperlegCOM;
+ Type_fr_trunk_X_RH_upperlegCOM fr_trunk_X_RH_upperlegCOM;
+ Type_fr_trunk_X_LF_lowerlegCOM fr_trunk_X_LF_lowerlegCOM;
+ Type_fr_trunk_X_RF_lowerlegCOM fr_trunk_X_RF_lowerlegCOM;
+ Type_fr_trunk_X_LH_lowerlegCOM fr_trunk_X_LH_lowerlegCOM;
+ Type_fr_trunk_X_RH_lowerlegCOM fr_trunk_X_RH_lowerlegCOM;
+ Type_fr_LF_foot_X_fr_LF_lowerleg fr_LF_foot_X_fr_LF_lowerleg;
+ Type_fr_RF_foot_X_fr_RF_lowerleg fr_RF_foot_X_fr_RF_lowerleg;
+ Type_fr_LH_foot_X_fr_LH_lowerleg fr_LH_foot_X_fr_LH_lowerleg;
+ Type_fr_RH_foot_X_fr_RH_lowerleg fr_RH_foot_X_fr_RH_lowerleg;
+ Type_fr_trunk_X_fr_LF_foot fr_trunk_X_fr_LF_foot;
+ Type_fr_trunk_X_fr_RF_foot fr_trunk_X_fr_RF_foot;
+ Type_fr_trunk_X_fr_LH_foot fr_trunk_X_fr_LH_foot;
+ Type_fr_trunk_X_fr_RH_foot fr_trunk_X_fr_RH_foot;
+ Type_fr_LF_foot_X_fr_trunk fr_LF_foot_X_fr_trunk;
+ Type_fr_RF_foot_X_fr_trunk fr_RF_foot_X_fr_trunk;
+ Type_fr_LH_foot_X_fr_trunk fr_LH_foot_X_fr_trunk;
+ Type_fr_RH_foot_X_fr_trunk fr_RH_foot_X_fr_trunk;
+ Type_fr_trunk_X_fr_LF_HAA fr_trunk_X_fr_LF_HAA;
+ Type_fr_trunk_X_fr_RF_HAA fr_trunk_X_fr_RF_HAA;
+ Type_fr_trunk_X_fr_LH_HAA fr_trunk_X_fr_LH_HAA;
+ Type_fr_trunk_X_fr_RH_HAA fr_trunk_X_fr_RH_HAA;
+ Type_fr_trunk_X_fr_LF_HFE fr_trunk_X_fr_LF_HFE;
+ Type_fr_trunk_X_fr_RF_HFE fr_trunk_X_fr_RF_HFE;
+ Type_fr_trunk_X_fr_LH_HFE fr_trunk_X_fr_LH_HFE;
+ Type_fr_trunk_X_fr_RH_HFE fr_trunk_X_fr_RH_HFE;
+ Type_fr_trunk_X_fr_LF_KFE fr_trunk_X_fr_LF_KFE;
+ Type_fr_trunk_X_fr_RF_KFE fr_trunk_X_fr_RF_KFE;
+ Type_fr_trunk_X_fr_LH_KFE fr_trunk_X_fr_LH_KFE;
+ Type_fr_trunk_X_fr_RH_KFE fr_trunk_X_fr_RH_KFE;
+ Type_fr_LF_upperleg_X_fr_LF_hipassembly fr_LF_upperleg_X_fr_LF_hipassembly;
+ Type_fr_LF_hipassembly_X_fr_LF_upperleg fr_LF_hipassembly_X_fr_LF_upperleg;
+ Type_fr_LF_lowerleg_X_fr_LF_upperleg fr_LF_lowerleg_X_fr_LF_upperleg;
+ Type_fr_LF_upperleg_X_fr_LF_lowerleg fr_LF_upperleg_X_fr_LF_lowerleg;
+ Type_fr_RF_upperleg_X_fr_RF_hipassembly fr_RF_upperleg_X_fr_RF_hipassembly;
+ Type_fr_RF_hipassembly_X_fr_RF_upperleg fr_RF_hipassembly_X_fr_RF_upperleg;
+ Type_fr_RF_lowerleg_X_fr_RF_upperleg fr_RF_lowerleg_X_fr_RF_upperleg;
+ Type_fr_RF_upperleg_X_fr_RF_lowerleg fr_RF_upperleg_X_fr_RF_lowerleg;
+ Type_fr_LH_upperleg_X_fr_LH_hipassembly fr_LH_upperleg_X_fr_LH_hipassembly;
+ Type_fr_LH_hipassembly_X_fr_LH_upperleg fr_LH_hipassembly_X_fr_LH_upperleg;
+ Type_fr_LH_lowerleg_X_fr_LH_upperleg fr_LH_lowerleg_X_fr_LH_upperleg;
+ Type_fr_LH_upperleg_X_fr_LH_lowerleg fr_LH_upperleg_X_fr_LH_lowerleg;
+ Type_fr_RH_upperleg_X_fr_RH_hipassembly fr_RH_upperleg_X_fr_RH_hipassembly;
+ Type_fr_RH_hipassembly_X_fr_RH_upperleg fr_RH_hipassembly_X_fr_RH_upperleg;
+ Type_fr_RH_lowerleg_X_fr_RH_upperleg fr_RH_lowerleg_X_fr_RH_upperleg;
+ Type_fr_RH_upperleg_X_fr_RH_lowerleg fr_RH_upperleg_X_fr_RH_lowerleg;
+
+protected:
+
+}; //class 'MotionTransforms'
+
+/**
+ * The class for the 6-by-6 coordinates transformation matrices for
+ * spatial force vectors.
+ */
+template <typename TRAIT>
+class ForceTransforms {
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef typename TRAIT::Scalar Scalar;
+
+ typedef JointState<Scalar> JState;
+ class Dummy {};
+ typedef typename TransformForce<Scalar, Dummy>::MatrixType MatrixType;
+public:
+ class Type_fr_trunk_X_fr_LF_hipassembly : public TransformForce<Scalar, Type_fr_trunk_X_fr_LF_hipassembly>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LF_hipassembly();
+ const Type_fr_trunk_X_fr_LF_hipassembly& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RF_hipassembly : public TransformForce<Scalar, Type_fr_trunk_X_fr_RF_hipassembly>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RF_hipassembly();
+ const Type_fr_trunk_X_fr_RF_hipassembly& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LH_hipassembly : public TransformForce<Scalar, Type_fr_trunk_X_fr_LH_hipassembly>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LH_hipassembly();
+ const Type_fr_trunk_X_fr_LH_hipassembly& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RH_hipassembly : public TransformForce<Scalar, Type_fr_trunk_X_fr_RH_hipassembly>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RH_hipassembly();
+ const Type_fr_trunk_X_fr_RH_hipassembly& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LF_upperleg : public TransformForce<Scalar, Type_fr_trunk_X_fr_LF_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LF_upperleg();
+ const Type_fr_trunk_X_fr_LF_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RF_upperleg : public TransformForce<Scalar, Type_fr_trunk_X_fr_RF_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RF_upperleg();
+ const Type_fr_trunk_X_fr_RF_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LH_upperleg : public TransformForce<Scalar, Type_fr_trunk_X_fr_LH_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LH_upperleg();
+ const Type_fr_trunk_X_fr_LH_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RH_upperleg : public TransformForce<Scalar, Type_fr_trunk_X_fr_RH_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RH_upperleg();
+ const Type_fr_trunk_X_fr_RH_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LF_lowerleg : public TransformForce<Scalar, Type_fr_trunk_X_fr_LF_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LF_lowerleg();
+ const Type_fr_trunk_X_fr_LF_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RF_lowerleg : public TransformForce<Scalar, Type_fr_trunk_X_fr_RF_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RF_lowerleg();
+ const Type_fr_trunk_X_fr_RF_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LH_lowerleg : public TransformForce<Scalar, Type_fr_trunk_X_fr_LH_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LH_lowerleg();
+ const Type_fr_trunk_X_fr_LH_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RH_lowerleg : public TransformForce<Scalar, Type_fr_trunk_X_fr_RH_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RH_lowerleg();
+ const Type_fr_trunk_X_fr_RH_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LF_hipassembly_X_fr_trunk : public TransformForce<Scalar, Type_fr_LF_hipassembly_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LF_hipassembly_X_fr_trunk();
+ const Type_fr_LF_hipassembly_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RF_hipassembly_X_fr_trunk : public TransformForce<Scalar, Type_fr_RF_hipassembly_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RF_hipassembly_X_fr_trunk();
+ const Type_fr_RF_hipassembly_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LH_hipassembly_X_fr_trunk : public TransformForce<Scalar, Type_fr_LH_hipassembly_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LH_hipassembly_X_fr_trunk();
+ const Type_fr_LH_hipassembly_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RH_hipassembly_X_fr_trunk : public TransformForce<Scalar, Type_fr_RH_hipassembly_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RH_hipassembly_X_fr_trunk();
+ const Type_fr_RH_hipassembly_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LF_upperleg_X_fr_trunk : public TransformForce<Scalar, Type_fr_LF_upperleg_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LF_upperleg_X_fr_trunk();
+ const Type_fr_LF_upperleg_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RF_upperleg_X_fr_trunk : public TransformForce<Scalar, Type_fr_RF_upperleg_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RF_upperleg_X_fr_trunk();
+ const Type_fr_RF_upperleg_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LH_upperleg_X_fr_trunk : public TransformForce<Scalar, Type_fr_LH_upperleg_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LH_upperleg_X_fr_trunk();
+ const Type_fr_LH_upperleg_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RH_upperleg_X_fr_trunk : public TransformForce<Scalar, Type_fr_RH_upperleg_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RH_upperleg_X_fr_trunk();
+ const Type_fr_RH_upperleg_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LF_lowerleg_X_fr_trunk : public TransformForce<Scalar, Type_fr_LF_lowerleg_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LF_lowerleg_X_fr_trunk();
+ const Type_fr_LF_lowerleg_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RF_lowerleg_X_fr_trunk : public TransformForce<Scalar, Type_fr_RF_lowerleg_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RF_lowerleg_X_fr_trunk();
+ const Type_fr_RF_lowerleg_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LH_lowerleg_X_fr_trunk : public TransformForce<Scalar, Type_fr_LH_lowerleg_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LH_lowerleg_X_fr_trunk();
+ const Type_fr_LH_lowerleg_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RH_lowerleg_X_fr_trunk : public TransformForce<Scalar, Type_fr_RH_lowerleg_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RH_lowerleg_X_fr_trunk();
+ const Type_fr_RH_lowerleg_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_LF_hipassemblyCOM : public TransformForce<Scalar, Type_fr_trunk_X_LF_hipassemblyCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_LF_hipassemblyCOM();
+ const Type_fr_trunk_X_LF_hipassemblyCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_RF_hipassemblyCOM : public TransformForce<Scalar, Type_fr_trunk_X_RF_hipassemblyCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_RF_hipassemblyCOM();
+ const Type_fr_trunk_X_RF_hipassemblyCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_LH_hipassemblyCOM : public TransformForce<Scalar, Type_fr_trunk_X_LH_hipassemblyCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_LH_hipassemblyCOM();
+ const Type_fr_trunk_X_LH_hipassemblyCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_RH_hipassemblyCOM : public TransformForce<Scalar, Type_fr_trunk_X_RH_hipassemblyCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_RH_hipassemblyCOM();
+ const Type_fr_trunk_X_RH_hipassemblyCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_LF_upperlegCOM : public TransformForce<Scalar, Type_fr_trunk_X_LF_upperlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_LF_upperlegCOM();
+ const Type_fr_trunk_X_LF_upperlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_RF_upperlegCOM : public TransformForce<Scalar, Type_fr_trunk_X_RF_upperlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_RF_upperlegCOM();
+ const Type_fr_trunk_X_RF_upperlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_LH_upperlegCOM : public TransformForce<Scalar, Type_fr_trunk_X_LH_upperlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_LH_upperlegCOM();
+ const Type_fr_trunk_X_LH_upperlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_RH_upperlegCOM : public TransformForce<Scalar, Type_fr_trunk_X_RH_upperlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_RH_upperlegCOM();
+ const Type_fr_trunk_X_RH_upperlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_LF_lowerlegCOM : public TransformForce<Scalar, Type_fr_trunk_X_LF_lowerlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_LF_lowerlegCOM();
+ const Type_fr_trunk_X_LF_lowerlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_RF_lowerlegCOM : public TransformForce<Scalar, Type_fr_trunk_X_RF_lowerlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_RF_lowerlegCOM();
+ const Type_fr_trunk_X_RF_lowerlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_LH_lowerlegCOM : public TransformForce<Scalar, Type_fr_trunk_X_LH_lowerlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_LH_lowerlegCOM();
+ const Type_fr_trunk_X_LH_lowerlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_RH_lowerlegCOM : public TransformForce<Scalar, Type_fr_trunk_X_RH_lowerlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_RH_lowerlegCOM();
+ const Type_fr_trunk_X_RH_lowerlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LF_foot_X_fr_LF_lowerleg : public TransformForce<Scalar, Type_fr_LF_foot_X_fr_LF_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LF_foot_X_fr_LF_lowerleg();
+ const Type_fr_LF_foot_X_fr_LF_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RF_foot_X_fr_RF_lowerleg : public TransformForce<Scalar, Type_fr_RF_foot_X_fr_RF_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RF_foot_X_fr_RF_lowerleg();
+ const Type_fr_RF_foot_X_fr_RF_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LH_foot_X_fr_LH_lowerleg : public TransformForce<Scalar, Type_fr_LH_foot_X_fr_LH_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LH_foot_X_fr_LH_lowerleg();
+ const Type_fr_LH_foot_X_fr_LH_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RH_foot_X_fr_RH_lowerleg : public TransformForce<Scalar, Type_fr_RH_foot_X_fr_RH_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RH_foot_X_fr_RH_lowerleg();
+ const Type_fr_RH_foot_X_fr_RH_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LF_foot : public TransformForce<Scalar, Type_fr_trunk_X_fr_LF_foot>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LF_foot();
+ const Type_fr_trunk_X_fr_LF_foot& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RF_foot : public TransformForce<Scalar, Type_fr_trunk_X_fr_RF_foot>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RF_foot();
+ const Type_fr_trunk_X_fr_RF_foot& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LH_foot : public TransformForce<Scalar, Type_fr_trunk_X_fr_LH_foot>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LH_foot();
+ const Type_fr_trunk_X_fr_LH_foot& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RH_foot : public TransformForce<Scalar, Type_fr_trunk_X_fr_RH_foot>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RH_foot();
+ const Type_fr_trunk_X_fr_RH_foot& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LF_foot_X_fr_trunk : public TransformForce<Scalar, Type_fr_LF_foot_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LF_foot_X_fr_trunk();
+ const Type_fr_LF_foot_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RF_foot_X_fr_trunk : public TransformForce<Scalar, Type_fr_RF_foot_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RF_foot_X_fr_trunk();
+ const Type_fr_RF_foot_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LH_foot_X_fr_trunk : public TransformForce<Scalar, Type_fr_LH_foot_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LH_foot_X_fr_trunk();
+ const Type_fr_LH_foot_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RH_foot_X_fr_trunk : public TransformForce<Scalar, Type_fr_RH_foot_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RH_foot_X_fr_trunk();
+ const Type_fr_RH_foot_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LF_HAA : public TransformForce<Scalar, Type_fr_trunk_X_fr_LF_HAA>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LF_HAA();
+ const Type_fr_trunk_X_fr_LF_HAA& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RF_HAA : public TransformForce<Scalar, Type_fr_trunk_X_fr_RF_HAA>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RF_HAA();
+ const Type_fr_trunk_X_fr_RF_HAA& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LH_HAA : public TransformForce<Scalar, Type_fr_trunk_X_fr_LH_HAA>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LH_HAA();
+ const Type_fr_trunk_X_fr_LH_HAA& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RH_HAA : public TransformForce<Scalar, Type_fr_trunk_X_fr_RH_HAA>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RH_HAA();
+ const Type_fr_trunk_X_fr_RH_HAA& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LF_HFE : public TransformForce<Scalar, Type_fr_trunk_X_fr_LF_HFE>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LF_HFE();
+ const Type_fr_trunk_X_fr_LF_HFE& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RF_HFE : public TransformForce<Scalar, Type_fr_trunk_X_fr_RF_HFE>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RF_HFE();
+ const Type_fr_trunk_X_fr_RF_HFE& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LH_HFE : public TransformForce<Scalar, Type_fr_trunk_X_fr_LH_HFE>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LH_HFE();
+ const Type_fr_trunk_X_fr_LH_HFE& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RH_HFE : public TransformForce<Scalar, Type_fr_trunk_X_fr_RH_HFE>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RH_HFE();
+ const Type_fr_trunk_X_fr_RH_HFE& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LF_KFE : public TransformForce<Scalar, Type_fr_trunk_X_fr_LF_KFE>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LF_KFE();
+ const Type_fr_trunk_X_fr_LF_KFE& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RF_KFE : public TransformForce<Scalar, Type_fr_trunk_X_fr_RF_KFE>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RF_KFE();
+ const Type_fr_trunk_X_fr_RF_KFE& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LH_KFE : public TransformForce<Scalar, Type_fr_trunk_X_fr_LH_KFE>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LH_KFE();
+ const Type_fr_trunk_X_fr_LH_KFE& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RH_KFE : public TransformForce<Scalar, Type_fr_trunk_X_fr_RH_KFE>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RH_KFE();
+ const Type_fr_trunk_X_fr_RH_KFE& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LF_upperleg_X_fr_LF_hipassembly : public TransformForce<Scalar, Type_fr_LF_upperleg_X_fr_LF_hipassembly>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LF_upperleg_X_fr_LF_hipassembly();
+ const Type_fr_LF_upperleg_X_fr_LF_hipassembly& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LF_hipassembly_X_fr_LF_upperleg : public TransformForce<Scalar, Type_fr_LF_hipassembly_X_fr_LF_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LF_hipassembly_X_fr_LF_upperleg();
+ const Type_fr_LF_hipassembly_X_fr_LF_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LF_lowerleg_X_fr_LF_upperleg : public TransformForce<Scalar, Type_fr_LF_lowerleg_X_fr_LF_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LF_lowerleg_X_fr_LF_upperleg();
+ const Type_fr_LF_lowerleg_X_fr_LF_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LF_upperleg_X_fr_LF_lowerleg : public TransformForce<Scalar, Type_fr_LF_upperleg_X_fr_LF_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LF_upperleg_X_fr_LF_lowerleg();
+ const Type_fr_LF_upperleg_X_fr_LF_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RF_upperleg_X_fr_RF_hipassembly : public TransformForce<Scalar, Type_fr_RF_upperleg_X_fr_RF_hipassembly>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RF_upperleg_X_fr_RF_hipassembly();
+ const Type_fr_RF_upperleg_X_fr_RF_hipassembly& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RF_hipassembly_X_fr_RF_upperleg : public TransformForce<Scalar, Type_fr_RF_hipassembly_X_fr_RF_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RF_hipassembly_X_fr_RF_upperleg();
+ const Type_fr_RF_hipassembly_X_fr_RF_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RF_lowerleg_X_fr_RF_upperleg : public TransformForce<Scalar, Type_fr_RF_lowerleg_X_fr_RF_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RF_lowerleg_X_fr_RF_upperleg();
+ const Type_fr_RF_lowerleg_X_fr_RF_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RF_upperleg_X_fr_RF_lowerleg : public TransformForce<Scalar, Type_fr_RF_upperleg_X_fr_RF_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RF_upperleg_X_fr_RF_lowerleg();
+ const Type_fr_RF_upperleg_X_fr_RF_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LH_upperleg_X_fr_LH_hipassembly : public TransformForce<Scalar, Type_fr_LH_upperleg_X_fr_LH_hipassembly>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LH_upperleg_X_fr_LH_hipassembly();
+ const Type_fr_LH_upperleg_X_fr_LH_hipassembly& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LH_hipassembly_X_fr_LH_upperleg : public TransformForce<Scalar, Type_fr_LH_hipassembly_X_fr_LH_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LH_hipassembly_X_fr_LH_upperleg();
+ const Type_fr_LH_hipassembly_X_fr_LH_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LH_lowerleg_X_fr_LH_upperleg : public TransformForce<Scalar, Type_fr_LH_lowerleg_X_fr_LH_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LH_lowerleg_X_fr_LH_upperleg();
+ const Type_fr_LH_lowerleg_X_fr_LH_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LH_upperleg_X_fr_LH_lowerleg : public TransformForce<Scalar, Type_fr_LH_upperleg_X_fr_LH_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LH_upperleg_X_fr_LH_lowerleg();
+ const Type_fr_LH_upperleg_X_fr_LH_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RH_upperleg_X_fr_RH_hipassembly : public TransformForce<Scalar, Type_fr_RH_upperleg_X_fr_RH_hipassembly>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RH_upperleg_X_fr_RH_hipassembly();
+ const Type_fr_RH_upperleg_X_fr_RH_hipassembly& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RH_hipassembly_X_fr_RH_upperleg : public TransformForce<Scalar, Type_fr_RH_hipassembly_X_fr_RH_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RH_hipassembly_X_fr_RH_upperleg();
+ const Type_fr_RH_hipassembly_X_fr_RH_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RH_lowerleg_X_fr_RH_upperleg : public TransformForce<Scalar, Type_fr_RH_lowerleg_X_fr_RH_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RH_lowerleg_X_fr_RH_upperleg();
+ const Type_fr_RH_lowerleg_X_fr_RH_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RH_upperleg_X_fr_RH_lowerleg : public TransformForce<Scalar, Type_fr_RH_upperleg_X_fr_RH_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RH_upperleg_X_fr_RH_lowerleg();
+ const Type_fr_RH_upperleg_X_fr_RH_lowerleg& update(const JState&);
+ protected:
+ };
+
+public:
+ ForceTransforms();
+ void updateParameters();
+ Type_fr_trunk_X_fr_LF_hipassembly fr_trunk_X_fr_LF_hipassembly;
+ Type_fr_trunk_X_fr_RF_hipassembly fr_trunk_X_fr_RF_hipassembly;
+ Type_fr_trunk_X_fr_LH_hipassembly fr_trunk_X_fr_LH_hipassembly;
+ Type_fr_trunk_X_fr_RH_hipassembly fr_trunk_X_fr_RH_hipassembly;
+ Type_fr_trunk_X_fr_LF_upperleg fr_trunk_X_fr_LF_upperleg;
+ Type_fr_trunk_X_fr_RF_upperleg fr_trunk_X_fr_RF_upperleg;
+ Type_fr_trunk_X_fr_LH_upperleg fr_trunk_X_fr_LH_upperleg;
+ Type_fr_trunk_X_fr_RH_upperleg fr_trunk_X_fr_RH_upperleg;
+ Type_fr_trunk_X_fr_LF_lowerleg fr_trunk_X_fr_LF_lowerleg;
+ Type_fr_trunk_X_fr_RF_lowerleg fr_trunk_X_fr_RF_lowerleg;
+ Type_fr_trunk_X_fr_LH_lowerleg fr_trunk_X_fr_LH_lowerleg;
+ Type_fr_trunk_X_fr_RH_lowerleg fr_trunk_X_fr_RH_lowerleg;
+ Type_fr_LF_hipassembly_X_fr_trunk fr_LF_hipassembly_X_fr_trunk;
+ Type_fr_RF_hipassembly_X_fr_trunk fr_RF_hipassembly_X_fr_trunk;
+ Type_fr_LH_hipassembly_X_fr_trunk fr_LH_hipassembly_X_fr_trunk;
+ Type_fr_RH_hipassembly_X_fr_trunk fr_RH_hipassembly_X_fr_trunk;
+ Type_fr_LF_upperleg_X_fr_trunk fr_LF_upperleg_X_fr_trunk;
+ Type_fr_RF_upperleg_X_fr_trunk fr_RF_upperleg_X_fr_trunk;
+ Type_fr_LH_upperleg_X_fr_trunk fr_LH_upperleg_X_fr_trunk;
+ Type_fr_RH_upperleg_X_fr_trunk fr_RH_upperleg_X_fr_trunk;
+ Type_fr_LF_lowerleg_X_fr_trunk fr_LF_lowerleg_X_fr_trunk;
+ Type_fr_RF_lowerleg_X_fr_trunk fr_RF_lowerleg_X_fr_trunk;
+ Type_fr_LH_lowerleg_X_fr_trunk fr_LH_lowerleg_X_fr_trunk;
+ Type_fr_RH_lowerleg_X_fr_trunk fr_RH_lowerleg_X_fr_trunk;
+ Type_fr_trunk_X_LF_hipassemblyCOM fr_trunk_X_LF_hipassemblyCOM;
+ Type_fr_trunk_X_RF_hipassemblyCOM fr_trunk_X_RF_hipassemblyCOM;
+ Type_fr_trunk_X_LH_hipassemblyCOM fr_trunk_X_LH_hipassemblyCOM;
+ Type_fr_trunk_X_RH_hipassemblyCOM fr_trunk_X_RH_hipassemblyCOM;
+ Type_fr_trunk_X_LF_upperlegCOM fr_trunk_X_LF_upperlegCOM;
+ Type_fr_trunk_X_RF_upperlegCOM fr_trunk_X_RF_upperlegCOM;
+ Type_fr_trunk_X_LH_upperlegCOM fr_trunk_X_LH_upperlegCOM;
+ Type_fr_trunk_X_RH_upperlegCOM fr_trunk_X_RH_upperlegCOM;
+ Type_fr_trunk_X_LF_lowerlegCOM fr_trunk_X_LF_lowerlegCOM;
+ Type_fr_trunk_X_RF_lowerlegCOM fr_trunk_X_RF_lowerlegCOM;
+ Type_fr_trunk_X_LH_lowerlegCOM fr_trunk_X_LH_lowerlegCOM;
+ Type_fr_trunk_X_RH_lowerlegCOM fr_trunk_X_RH_lowerlegCOM;
+ Type_fr_LF_foot_X_fr_LF_lowerleg fr_LF_foot_X_fr_LF_lowerleg;
+ Type_fr_RF_foot_X_fr_RF_lowerleg fr_RF_foot_X_fr_RF_lowerleg;
+ Type_fr_LH_foot_X_fr_LH_lowerleg fr_LH_foot_X_fr_LH_lowerleg;
+ Type_fr_RH_foot_X_fr_RH_lowerleg fr_RH_foot_X_fr_RH_lowerleg;
+ Type_fr_trunk_X_fr_LF_foot fr_trunk_X_fr_LF_foot;
+ Type_fr_trunk_X_fr_RF_foot fr_trunk_X_fr_RF_foot;
+ Type_fr_trunk_X_fr_LH_foot fr_trunk_X_fr_LH_foot;
+ Type_fr_trunk_X_fr_RH_foot fr_trunk_X_fr_RH_foot;
+ Type_fr_LF_foot_X_fr_trunk fr_LF_foot_X_fr_trunk;
+ Type_fr_RF_foot_X_fr_trunk fr_RF_foot_X_fr_trunk;
+ Type_fr_LH_foot_X_fr_trunk fr_LH_foot_X_fr_trunk;
+ Type_fr_RH_foot_X_fr_trunk fr_RH_foot_X_fr_trunk;
+ Type_fr_trunk_X_fr_LF_HAA fr_trunk_X_fr_LF_HAA;
+ Type_fr_trunk_X_fr_RF_HAA fr_trunk_X_fr_RF_HAA;
+ Type_fr_trunk_X_fr_LH_HAA fr_trunk_X_fr_LH_HAA;
+ Type_fr_trunk_X_fr_RH_HAA fr_trunk_X_fr_RH_HAA;
+ Type_fr_trunk_X_fr_LF_HFE fr_trunk_X_fr_LF_HFE;
+ Type_fr_trunk_X_fr_RF_HFE fr_trunk_X_fr_RF_HFE;
+ Type_fr_trunk_X_fr_LH_HFE fr_trunk_X_fr_LH_HFE;
+ Type_fr_trunk_X_fr_RH_HFE fr_trunk_X_fr_RH_HFE;
+ Type_fr_trunk_X_fr_LF_KFE fr_trunk_X_fr_LF_KFE;
+ Type_fr_trunk_X_fr_RF_KFE fr_trunk_X_fr_RF_KFE;
+ Type_fr_trunk_X_fr_LH_KFE fr_trunk_X_fr_LH_KFE;
+ Type_fr_trunk_X_fr_RH_KFE fr_trunk_X_fr_RH_KFE;
+ Type_fr_LF_upperleg_X_fr_LF_hipassembly fr_LF_upperleg_X_fr_LF_hipassembly;
+ Type_fr_LF_hipassembly_X_fr_LF_upperleg fr_LF_hipassembly_X_fr_LF_upperleg;
+ Type_fr_LF_lowerleg_X_fr_LF_upperleg fr_LF_lowerleg_X_fr_LF_upperleg;
+ Type_fr_LF_upperleg_X_fr_LF_lowerleg fr_LF_upperleg_X_fr_LF_lowerleg;
+ Type_fr_RF_upperleg_X_fr_RF_hipassembly fr_RF_upperleg_X_fr_RF_hipassembly;
+ Type_fr_RF_hipassembly_X_fr_RF_upperleg fr_RF_hipassembly_X_fr_RF_upperleg;
+ Type_fr_RF_lowerleg_X_fr_RF_upperleg fr_RF_lowerleg_X_fr_RF_upperleg;
+ Type_fr_RF_upperleg_X_fr_RF_lowerleg fr_RF_upperleg_X_fr_RF_lowerleg;
+ Type_fr_LH_upperleg_X_fr_LH_hipassembly fr_LH_upperleg_X_fr_LH_hipassembly;
+ Type_fr_LH_hipassembly_X_fr_LH_upperleg fr_LH_hipassembly_X_fr_LH_upperleg;
+ Type_fr_LH_lowerleg_X_fr_LH_upperleg fr_LH_lowerleg_X_fr_LH_upperleg;
+ Type_fr_LH_upperleg_X_fr_LH_lowerleg fr_LH_upperleg_X_fr_LH_lowerleg;
+ Type_fr_RH_upperleg_X_fr_RH_hipassembly fr_RH_upperleg_X_fr_RH_hipassembly;
+ Type_fr_RH_hipassembly_X_fr_RH_upperleg fr_RH_hipassembly_X_fr_RH_upperleg;
+ Type_fr_RH_lowerleg_X_fr_RH_upperleg fr_RH_lowerleg_X_fr_RH_upperleg;
+ Type_fr_RH_upperleg_X_fr_RH_lowerleg fr_RH_upperleg_X_fr_RH_lowerleg;
+
+protected:
+
+}; //class 'ForceTransforms'
+
+/**
+ * The class with the homogeneous (4x4) coordinates transformation
+ * matrices.
+ */
+template <typename TRAIT>
+class HomogeneousTransforms {
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef typename TRAIT::Scalar Scalar;
+
+ typedef JointState<Scalar> JState;
+ class Dummy {};
+ typedef typename TransformHomogeneous<Scalar, Dummy>::MatrixType MatrixType;
+public:
+ class Type_fr_trunk_X_fr_LF_hipassembly : public TransformHomogeneous<Scalar, Type_fr_trunk_X_fr_LF_hipassembly>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LF_hipassembly();
+ const Type_fr_trunk_X_fr_LF_hipassembly& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RF_hipassembly : public TransformHomogeneous<Scalar, Type_fr_trunk_X_fr_RF_hipassembly>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RF_hipassembly();
+ const Type_fr_trunk_X_fr_RF_hipassembly& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LH_hipassembly : public TransformHomogeneous<Scalar, Type_fr_trunk_X_fr_LH_hipassembly>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LH_hipassembly();
+ const Type_fr_trunk_X_fr_LH_hipassembly& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RH_hipassembly : public TransformHomogeneous<Scalar, Type_fr_trunk_X_fr_RH_hipassembly>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RH_hipassembly();
+ const Type_fr_trunk_X_fr_RH_hipassembly& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LF_upperleg : public TransformHomogeneous<Scalar, Type_fr_trunk_X_fr_LF_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LF_upperleg();
+ const Type_fr_trunk_X_fr_LF_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RF_upperleg : public TransformHomogeneous<Scalar, Type_fr_trunk_X_fr_RF_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RF_upperleg();
+ const Type_fr_trunk_X_fr_RF_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LH_upperleg : public TransformHomogeneous<Scalar, Type_fr_trunk_X_fr_LH_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LH_upperleg();
+ const Type_fr_trunk_X_fr_LH_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RH_upperleg : public TransformHomogeneous<Scalar, Type_fr_trunk_X_fr_RH_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RH_upperleg();
+ const Type_fr_trunk_X_fr_RH_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LF_lowerleg : public TransformHomogeneous<Scalar, Type_fr_trunk_X_fr_LF_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LF_lowerleg();
+ const Type_fr_trunk_X_fr_LF_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RF_lowerleg : public TransformHomogeneous<Scalar, Type_fr_trunk_X_fr_RF_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RF_lowerleg();
+ const Type_fr_trunk_X_fr_RF_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LH_lowerleg : public TransformHomogeneous<Scalar, Type_fr_trunk_X_fr_LH_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LH_lowerleg();
+ const Type_fr_trunk_X_fr_LH_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RH_lowerleg : public TransformHomogeneous<Scalar, Type_fr_trunk_X_fr_RH_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RH_lowerleg();
+ const Type_fr_trunk_X_fr_RH_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LF_hipassembly_X_fr_trunk : public TransformHomogeneous<Scalar, Type_fr_LF_hipassembly_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LF_hipassembly_X_fr_trunk();
+ const Type_fr_LF_hipassembly_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RF_hipassembly_X_fr_trunk : public TransformHomogeneous<Scalar, Type_fr_RF_hipassembly_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RF_hipassembly_X_fr_trunk();
+ const Type_fr_RF_hipassembly_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LH_hipassembly_X_fr_trunk : public TransformHomogeneous<Scalar, Type_fr_LH_hipassembly_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LH_hipassembly_X_fr_trunk();
+ const Type_fr_LH_hipassembly_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RH_hipassembly_X_fr_trunk : public TransformHomogeneous<Scalar, Type_fr_RH_hipassembly_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RH_hipassembly_X_fr_trunk();
+ const Type_fr_RH_hipassembly_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LF_upperleg_X_fr_trunk : public TransformHomogeneous<Scalar, Type_fr_LF_upperleg_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LF_upperleg_X_fr_trunk();
+ const Type_fr_LF_upperleg_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RF_upperleg_X_fr_trunk : public TransformHomogeneous<Scalar, Type_fr_RF_upperleg_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RF_upperleg_X_fr_trunk();
+ const Type_fr_RF_upperleg_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LH_upperleg_X_fr_trunk : public TransformHomogeneous<Scalar, Type_fr_LH_upperleg_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LH_upperleg_X_fr_trunk();
+ const Type_fr_LH_upperleg_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RH_upperleg_X_fr_trunk : public TransformHomogeneous<Scalar, Type_fr_RH_upperleg_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RH_upperleg_X_fr_trunk();
+ const Type_fr_RH_upperleg_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LF_lowerleg_X_fr_trunk : public TransformHomogeneous<Scalar, Type_fr_LF_lowerleg_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LF_lowerleg_X_fr_trunk();
+ const Type_fr_LF_lowerleg_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RF_lowerleg_X_fr_trunk : public TransformHomogeneous<Scalar, Type_fr_RF_lowerleg_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RF_lowerleg_X_fr_trunk();
+ const Type_fr_RF_lowerleg_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LH_lowerleg_X_fr_trunk : public TransformHomogeneous<Scalar, Type_fr_LH_lowerleg_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LH_lowerleg_X_fr_trunk();
+ const Type_fr_LH_lowerleg_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RH_lowerleg_X_fr_trunk : public TransformHomogeneous<Scalar, Type_fr_RH_lowerleg_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RH_lowerleg_X_fr_trunk();
+ const Type_fr_RH_lowerleg_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_LF_hipassemblyCOM : public TransformHomogeneous<Scalar, Type_fr_trunk_X_LF_hipassemblyCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_LF_hipassemblyCOM();
+ const Type_fr_trunk_X_LF_hipassemblyCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_RF_hipassemblyCOM : public TransformHomogeneous<Scalar, Type_fr_trunk_X_RF_hipassemblyCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_RF_hipassemblyCOM();
+ const Type_fr_trunk_X_RF_hipassemblyCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_LH_hipassemblyCOM : public TransformHomogeneous<Scalar, Type_fr_trunk_X_LH_hipassemblyCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_LH_hipassemblyCOM();
+ const Type_fr_trunk_X_LH_hipassemblyCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_RH_hipassemblyCOM : public TransformHomogeneous<Scalar, Type_fr_trunk_X_RH_hipassemblyCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_RH_hipassemblyCOM();
+ const Type_fr_trunk_X_RH_hipassemblyCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_LF_upperlegCOM : public TransformHomogeneous<Scalar, Type_fr_trunk_X_LF_upperlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_LF_upperlegCOM();
+ const Type_fr_trunk_X_LF_upperlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_RF_upperlegCOM : public TransformHomogeneous<Scalar, Type_fr_trunk_X_RF_upperlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_RF_upperlegCOM();
+ const Type_fr_trunk_X_RF_upperlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_LH_upperlegCOM : public TransformHomogeneous<Scalar, Type_fr_trunk_X_LH_upperlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_LH_upperlegCOM();
+ const Type_fr_trunk_X_LH_upperlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_RH_upperlegCOM : public TransformHomogeneous<Scalar, Type_fr_trunk_X_RH_upperlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_RH_upperlegCOM();
+ const Type_fr_trunk_X_RH_upperlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_LF_lowerlegCOM : public TransformHomogeneous<Scalar, Type_fr_trunk_X_LF_lowerlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_LF_lowerlegCOM();
+ const Type_fr_trunk_X_LF_lowerlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_RF_lowerlegCOM : public TransformHomogeneous<Scalar, Type_fr_trunk_X_RF_lowerlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_RF_lowerlegCOM();
+ const Type_fr_trunk_X_RF_lowerlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_LH_lowerlegCOM : public TransformHomogeneous<Scalar, Type_fr_trunk_X_LH_lowerlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_LH_lowerlegCOM();
+ const Type_fr_trunk_X_LH_lowerlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_RH_lowerlegCOM : public TransformHomogeneous<Scalar, Type_fr_trunk_X_RH_lowerlegCOM>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_RH_lowerlegCOM();
+ const Type_fr_trunk_X_RH_lowerlegCOM& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LF_foot_X_fr_LF_lowerleg : public TransformHomogeneous<Scalar, Type_fr_LF_foot_X_fr_LF_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LF_foot_X_fr_LF_lowerleg();
+ const Type_fr_LF_foot_X_fr_LF_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RF_foot_X_fr_RF_lowerleg : public TransformHomogeneous<Scalar, Type_fr_RF_foot_X_fr_RF_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RF_foot_X_fr_RF_lowerleg();
+ const Type_fr_RF_foot_X_fr_RF_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LH_foot_X_fr_LH_lowerleg : public TransformHomogeneous<Scalar, Type_fr_LH_foot_X_fr_LH_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LH_foot_X_fr_LH_lowerleg();
+ const Type_fr_LH_foot_X_fr_LH_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RH_foot_X_fr_RH_lowerleg : public TransformHomogeneous<Scalar, Type_fr_RH_foot_X_fr_RH_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RH_foot_X_fr_RH_lowerleg();
+ const Type_fr_RH_foot_X_fr_RH_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LF_foot : public TransformHomogeneous<Scalar, Type_fr_trunk_X_fr_LF_foot>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LF_foot();
+ const Type_fr_trunk_X_fr_LF_foot& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RF_foot : public TransformHomogeneous<Scalar, Type_fr_trunk_X_fr_RF_foot>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RF_foot();
+ const Type_fr_trunk_X_fr_RF_foot& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LH_foot : public TransformHomogeneous<Scalar, Type_fr_trunk_X_fr_LH_foot>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LH_foot();
+ const Type_fr_trunk_X_fr_LH_foot& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RH_foot : public TransformHomogeneous<Scalar, Type_fr_trunk_X_fr_RH_foot>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RH_foot();
+ const Type_fr_trunk_X_fr_RH_foot& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LF_foot_X_fr_trunk : public TransformHomogeneous<Scalar, Type_fr_LF_foot_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LF_foot_X_fr_trunk();
+ const Type_fr_LF_foot_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RF_foot_X_fr_trunk : public TransformHomogeneous<Scalar, Type_fr_RF_foot_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RF_foot_X_fr_trunk();
+ const Type_fr_RF_foot_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LH_foot_X_fr_trunk : public TransformHomogeneous<Scalar, Type_fr_LH_foot_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LH_foot_X_fr_trunk();
+ const Type_fr_LH_foot_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RH_foot_X_fr_trunk : public TransformHomogeneous<Scalar, Type_fr_RH_foot_X_fr_trunk>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RH_foot_X_fr_trunk();
+ const Type_fr_RH_foot_X_fr_trunk& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LF_HAA : public TransformHomogeneous<Scalar, Type_fr_trunk_X_fr_LF_HAA>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LF_HAA();
+ const Type_fr_trunk_X_fr_LF_HAA& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RF_HAA : public TransformHomogeneous<Scalar, Type_fr_trunk_X_fr_RF_HAA>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RF_HAA();
+ const Type_fr_trunk_X_fr_RF_HAA& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LH_HAA : public TransformHomogeneous<Scalar, Type_fr_trunk_X_fr_LH_HAA>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LH_HAA();
+ const Type_fr_trunk_X_fr_LH_HAA& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RH_HAA : public TransformHomogeneous<Scalar, Type_fr_trunk_X_fr_RH_HAA>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RH_HAA();
+ const Type_fr_trunk_X_fr_RH_HAA& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LF_HFE : public TransformHomogeneous<Scalar, Type_fr_trunk_X_fr_LF_HFE>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LF_HFE();
+ const Type_fr_trunk_X_fr_LF_HFE& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RF_HFE : public TransformHomogeneous<Scalar, Type_fr_trunk_X_fr_RF_HFE>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RF_HFE();
+ const Type_fr_trunk_X_fr_RF_HFE& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LH_HFE : public TransformHomogeneous<Scalar, Type_fr_trunk_X_fr_LH_HFE>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LH_HFE();
+ const Type_fr_trunk_X_fr_LH_HFE& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RH_HFE : public TransformHomogeneous<Scalar, Type_fr_trunk_X_fr_RH_HFE>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RH_HFE();
+ const Type_fr_trunk_X_fr_RH_HFE& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LF_KFE : public TransformHomogeneous<Scalar, Type_fr_trunk_X_fr_LF_KFE>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LF_KFE();
+ const Type_fr_trunk_X_fr_LF_KFE& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RF_KFE : public TransformHomogeneous<Scalar, Type_fr_trunk_X_fr_RF_KFE>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RF_KFE();
+ const Type_fr_trunk_X_fr_RF_KFE& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_LH_KFE : public TransformHomogeneous<Scalar, Type_fr_trunk_X_fr_LH_KFE>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_LH_KFE();
+ const Type_fr_trunk_X_fr_LH_KFE& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_trunk_X_fr_RH_KFE : public TransformHomogeneous<Scalar, Type_fr_trunk_X_fr_RH_KFE>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_trunk_X_fr_RH_KFE();
+ const Type_fr_trunk_X_fr_RH_KFE& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LF_upperleg_X_fr_LF_hipassembly : public TransformHomogeneous<Scalar, Type_fr_LF_upperleg_X_fr_LF_hipassembly>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LF_upperleg_X_fr_LF_hipassembly();
+ const Type_fr_LF_upperleg_X_fr_LF_hipassembly& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LF_hipassembly_X_fr_LF_upperleg : public TransformHomogeneous<Scalar, Type_fr_LF_hipassembly_X_fr_LF_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LF_hipassembly_X_fr_LF_upperleg();
+ const Type_fr_LF_hipassembly_X_fr_LF_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LF_lowerleg_X_fr_LF_upperleg : public TransformHomogeneous<Scalar, Type_fr_LF_lowerleg_X_fr_LF_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LF_lowerleg_X_fr_LF_upperleg();
+ const Type_fr_LF_lowerleg_X_fr_LF_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LF_upperleg_X_fr_LF_lowerleg : public TransformHomogeneous<Scalar, Type_fr_LF_upperleg_X_fr_LF_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LF_upperleg_X_fr_LF_lowerleg();
+ const Type_fr_LF_upperleg_X_fr_LF_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RF_upperleg_X_fr_RF_hipassembly : public TransformHomogeneous<Scalar, Type_fr_RF_upperleg_X_fr_RF_hipassembly>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RF_upperleg_X_fr_RF_hipassembly();
+ const Type_fr_RF_upperleg_X_fr_RF_hipassembly& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RF_hipassembly_X_fr_RF_upperleg : public TransformHomogeneous<Scalar, Type_fr_RF_hipassembly_X_fr_RF_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RF_hipassembly_X_fr_RF_upperleg();
+ const Type_fr_RF_hipassembly_X_fr_RF_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RF_lowerleg_X_fr_RF_upperleg : public TransformHomogeneous<Scalar, Type_fr_RF_lowerleg_X_fr_RF_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RF_lowerleg_X_fr_RF_upperleg();
+ const Type_fr_RF_lowerleg_X_fr_RF_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RF_upperleg_X_fr_RF_lowerleg : public TransformHomogeneous<Scalar, Type_fr_RF_upperleg_X_fr_RF_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RF_upperleg_X_fr_RF_lowerleg();
+ const Type_fr_RF_upperleg_X_fr_RF_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LH_upperleg_X_fr_LH_hipassembly : public TransformHomogeneous<Scalar, Type_fr_LH_upperleg_X_fr_LH_hipassembly>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LH_upperleg_X_fr_LH_hipassembly();
+ const Type_fr_LH_upperleg_X_fr_LH_hipassembly& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LH_hipassembly_X_fr_LH_upperleg : public TransformHomogeneous<Scalar, Type_fr_LH_hipassembly_X_fr_LH_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LH_hipassembly_X_fr_LH_upperleg();
+ const Type_fr_LH_hipassembly_X_fr_LH_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LH_lowerleg_X_fr_LH_upperleg : public TransformHomogeneous<Scalar, Type_fr_LH_lowerleg_X_fr_LH_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LH_lowerleg_X_fr_LH_upperleg();
+ const Type_fr_LH_lowerleg_X_fr_LH_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_LH_upperleg_X_fr_LH_lowerleg : public TransformHomogeneous<Scalar, Type_fr_LH_upperleg_X_fr_LH_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_LH_upperleg_X_fr_LH_lowerleg();
+ const Type_fr_LH_upperleg_X_fr_LH_lowerleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RH_upperleg_X_fr_RH_hipassembly : public TransformHomogeneous<Scalar, Type_fr_RH_upperleg_X_fr_RH_hipassembly>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RH_upperleg_X_fr_RH_hipassembly();
+ const Type_fr_RH_upperleg_X_fr_RH_hipassembly& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RH_hipassembly_X_fr_RH_upperleg : public TransformHomogeneous<Scalar, Type_fr_RH_hipassembly_X_fr_RH_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RH_hipassembly_X_fr_RH_upperleg();
+ const Type_fr_RH_hipassembly_X_fr_RH_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RH_lowerleg_X_fr_RH_upperleg : public TransformHomogeneous<Scalar, Type_fr_RH_lowerleg_X_fr_RH_upperleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RH_lowerleg_X_fr_RH_upperleg();
+ const Type_fr_RH_lowerleg_X_fr_RH_upperleg& update(const JState&);
+ protected:
+ };
+
+ class Type_fr_RH_upperleg_X_fr_RH_lowerleg : public TransformHomogeneous<Scalar, Type_fr_RH_upperleg_X_fr_RH_lowerleg>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Type_fr_RH_upperleg_X_fr_RH_lowerleg();
+ const Type_fr_RH_upperleg_X_fr_RH_lowerleg& update(const JState&);
+ protected:
+ };
+
+public:
+ HomogeneousTransforms();
+ void updateParameters();
+ Type_fr_trunk_X_fr_LF_hipassembly fr_trunk_X_fr_LF_hipassembly;
+ Type_fr_trunk_X_fr_RF_hipassembly fr_trunk_X_fr_RF_hipassembly;
+ Type_fr_trunk_X_fr_LH_hipassembly fr_trunk_X_fr_LH_hipassembly;
+ Type_fr_trunk_X_fr_RH_hipassembly fr_trunk_X_fr_RH_hipassembly;
+ Type_fr_trunk_X_fr_LF_upperleg fr_trunk_X_fr_LF_upperleg;
+ Type_fr_trunk_X_fr_RF_upperleg fr_trunk_X_fr_RF_upperleg;
+ Type_fr_trunk_X_fr_LH_upperleg fr_trunk_X_fr_LH_upperleg;
+ Type_fr_trunk_X_fr_RH_upperleg fr_trunk_X_fr_RH_upperleg;
+ Type_fr_trunk_X_fr_LF_lowerleg fr_trunk_X_fr_LF_lowerleg;
+ Type_fr_trunk_X_fr_RF_lowerleg fr_trunk_X_fr_RF_lowerleg;
+ Type_fr_trunk_X_fr_LH_lowerleg fr_trunk_X_fr_LH_lowerleg;
+ Type_fr_trunk_X_fr_RH_lowerleg fr_trunk_X_fr_RH_lowerleg;
+ Type_fr_LF_hipassembly_X_fr_trunk fr_LF_hipassembly_X_fr_trunk;
+ Type_fr_RF_hipassembly_X_fr_trunk fr_RF_hipassembly_X_fr_trunk;
+ Type_fr_LH_hipassembly_X_fr_trunk fr_LH_hipassembly_X_fr_trunk;
+ Type_fr_RH_hipassembly_X_fr_trunk fr_RH_hipassembly_X_fr_trunk;
+ Type_fr_LF_upperleg_X_fr_trunk fr_LF_upperleg_X_fr_trunk;
+ Type_fr_RF_upperleg_X_fr_trunk fr_RF_upperleg_X_fr_trunk;
+ Type_fr_LH_upperleg_X_fr_trunk fr_LH_upperleg_X_fr_trunk;
+ Type_fr_RH_upperleg_X_fr_trunk fr_RH_upperleg_X_fr_trunk;
+ Type_fr_LF_lowerleg_X_fr_trunk fr_LF_lowerleg_X_fr_trunk;
+ Type_fr_RF_lowerleg_X_fr_trunk fr_RF_lowerleg_X_fr_trunk;
+ Type_fr_LH_lowerleg_X_fr_trunk fr_LH_lowerleg_X_fr_trunk;
+ Type_fr_RH_lowerleg_X_fr_trunk fr_RH_lowerleg_X_fr_trunk;
+ Type_fr_trunk_X_LF_hipassemblyCOM fr_trunk_X_LF_hipassemblyCOM;
+ Type_fr_trunk_X_RF_hipassemblyCOM fr_trunk_X_RF_hipassemblyCOM;
+ Type_fr_trunk_X_LH_hipassemblyCOM fr_trunk_X_LH_hipassemblyCOM;
+ Type_fr_trunk_X_RH_hipassemblyCOM fr_trunk_X_RH_hipassemblyCOM;
+ Type_fr_trunk_X_LF_upperlegCOM fr_trunk_X_LF_upperlegCOM;
+ Type_fr_trunk_X_RF_upperlegCOM fr_trunk_X_RF_upperlegCOM;
+ Type_fr_trunk_X_LH_upperlegCOM fr_trunk_X_LH_upperlegCOM;
+ Type_fr_trunk_X_RH_upperlegCOM fr_trunk_X_RH_upperlegCOM;
+ Type_fr_trunk_X_LF_lowerlegCOM fr_trunk_X_LF_lowerlegCOM;
+ Type_fr_trunk_X_RF_lowerlegCOM fr_trunk_X_RF_lowerlegCOM;
+ Type_fr_trunk_X_LH_lowerlegCOM fr_trunk_X_LH_lowerlegCOM;
+ Type_fr_trunk_X_RH_lowerlegCOM fr_trunk_X_RH_lowerlegCOM;
+ Type_fr_LF_foot_X_fr_LF_lowerleg fr_LF_foot_X_fr_LF_lowerleg;
+ Type_fr_RF_foot_X_fr_RF_lowerleg fr_RF_foot_X_fr_RF_lowerleg;
+ Type_fr_LH_foot_X_fr_LH_lowerleg fr_LH_foot_X_fr_LH_lowerleg;
+ Type_fr_RH_foot_X_fr_RH_lowerleg fr_RH_foot_X_fr_RH_lowerleg;
+ Type_fr_trunk_X_fr_LF_foot fr_trunk_X_fr_LF_foot;
+ Type_fr_trunk_X_fr_RF_foot fr_trunk_X_fr_RF_foot;
+ Type_fr_trunk_X_fr_LH_foot fr_trunk_X_fr_LH_foot;
+ Type_fr_trunk_X_fr_RH_foot fr_trunk_X_fr_RH_foot;
+ Type_fr_LF_foot_X_fr_trunk fr_LF_foot_X_fr_trunk;
+ Type_fr_RF_foot_X_fr_trunk fr_RF_foot_X_fr_trunk;
+ Type_fr_LH_foot_X_fr_trunk fr_LH_foot_X_fr_trunk;
+ Type_fr_RH_foot_X_fr_trunk fr_RH_foot_X_fr_trunk;
+ Type_fr_trunk_X_fr_LF_HAA fr_trunk_X_fr_LF_HAA;
+ Type_fr_trunk_X_fr_RF_HAA fr_trunk_X_fr_RF_HAA;
+ Type_fr_trunk_X_fr_LH_HAA fr_trunk_X_fr_LH_HAA;
+ Type_fr_trunk_X_fr_RH_HAA fr_trunk_X_fr_RH_HAA;
+ Type_fr_trunk_X_fr_LF_HFE fr_trunk_X_fr_LF_HFE;
+ Type_fr_trunk_X_fr_RF_HFE fr_trunk_X_fr_RF_HFE;
+ Type_fr_trunk_X_fr_LH_HFE fr_trunk_X_fr_LH_HFE;
+ Type_fr_trunk_X_fr_RH_HFE fr_trunk_X_fr_RH_HFE;
+ Type_fr_trunk_X_fr_LF_KFE fr_trunk_X_fr_LF_KFE;
+ Type_fr_trunk_X_fr_RF_KFE fr_trunk_X_fr_RF_KFE;
+ Type_fr_trunk_X_fr_LH_KFE fr_trunk_X_fr_LH_KFE;
+ Type_fr_trunk_X_fr_RH_KFE fr_trunk_X_fr_RH_KFE;
+ Type_fr_LF_upperleg_X_fr_LF_hipassembly fr_LF_upperleg_X_fr_LF_hipassembly;
+ Type_fr_LF_hipassembly_X_fr_LF_upperleg fr_LF_hipassembly_X_fr_LF_upperleg;
+ Type_fr_LF_lowerleg_X_fr_LF_upperleg fr_LF_lowerleg_X_fr_LF_upperleg;
+ Type_fr_LF_upperleg_X_fr_LF_lowerleg fr_LF_upperleg_X_fr_LF_lowerleg;
+ Type_fr_RF_upperleg_X_fr_RF_hipassembly fr_RF_upperleg_X_fr_RF_hipassembly;
+ Type_fr_RF_hipassembly_X_fr_RF_upperleg fr_RF_hipassembly_X_fr_RF_upperleg;
+ Type_fr_RF_lowerleg_X_fr_RF_upperleg fr_RF_lowerleg_X_fr_RF_upperleg;
+ Type_fr_RF_upperleg_X_fr_RF_lowerleg fr_RF_upperleg_X_fr_RF_lowerleg;
+ Type_fr_LH_upperleg_X_fr_LH_hipassembly fr_LH_upperleg_X_fr_LH_hipassembly;
+ Type_fr_LH_hipassembly_X_fr_LH_upperleg fr_LH_hipassembly_X_fr_LH_upperleg;
+ Type_fr_LH_lowerleg_X_fr_LH_upperleg fr_LH_lowerleg_X_fr_LH_upperleg;
+ Type_fr_LH_upperleg_X_fr_LH_lowerleg fr_LH_upperleg_X_fr_LH_lowerleg;
+ Type_fr_RH_upperleg_X_fr_RH_hipassembly fr_RH_upperleg_X_fr_RH_hipassembly;
+ Type_fr_RH_hipassembly_X_fr_RH_upperleg fr_RH_hipassembly_X_fr_RH_upperleg;
+ Type_fr_RH_lowerleg_X_fr_RH_upperleg fr_RH_lowerleg_X_fr_RH_upperleg;
+ Type_fr_RH_upperleg_X_fr_RH_lowerleg fr_RH_upperleg_X_fr_RH_lowerleg;
+
+protected:
+
+}; //class 'HomogeneousTransforms'
+
+}
+
+using MotionTransforms = tpl::MotionTransforms<rbd::DoubleTrait>;
+using ForceTransforms = tpl::ForceTransforms<rbd::DoubleTrait>;
+using HomogeneousTransforms = tpl::HomogeneousTransforms<rbd::DoubleTrait>;
+
+}
+}
+
+#include "transforms.impl.h"
+
+#endif
diff --git a/ct_rbd/test/models/testhyq/generated/transforms.impl.h b/ct_rbd/test/models/testhyq/generated/transforms.impl.h
new file mode 100644
index 0000000..de4eec3
--- /dev/null
+++ b/ct_rbd/test/models/testhyq/generated/transforms.impl.h
@@ -0,0 +1,10674 @@
+
+// Constructors
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::MotionTransforms
+ ()
+ :
+ fr_trunk_X_fr_LF_hipassembly(),
+ fr_trunk_X_fr_RF_hipassembly(),
+ fr_trunk_X_fr_LH_hipassembly(),
+ fr_trunk_X_fr_RH_hipassembly(),
+ fr_trunk_X_fr_LF_upperleg(),
+ fr_trunk_X_fr_RF_upperleg(),
+ fr_trunk_X_fr_LH_upperleg(),
+ fr_trunk_X_fr_RH_upperleg(),
+ fr_trunk_X_fr_LF_lowerleg(),
+ fr_trunk_X_fr_RF_lowerleg(),
+ fr_trunk_X_fr_LH_lowerleg(),
+ fr_trunk_X_fr_RH_lowerleg(),
+ fr_LF_hipassembly_X_fr_trunk(),
+ fr_RF_hipassembly_X_fr_trunk(),
+ fr_LH_hipassembly_X_fr_trunk(),
+ fr_RH_hipassembly_X_fr_trunk(),
+ fr_LF_upperleg_X_fr_trunk(),
+ fr_RF_upperleg_X_fr_trunk(),
+ fr_LH_upperleg_X_fr_trunk(),
+ fr_RH_upperleg_X_fr_trunk(),
+ fr_LF_lowerleg_X_fr_trunk(),
+ fr_RF_lowerleg_X_fr_trunk(),
+ fr_LH_lowerleg_X_fr_trunk(),
+ fr_RH_lowerleg_X_fr_trunk(),
+ fr_trunk_X_LF_hipassemblyCOM(),
+ fr_trunk_X_RF_hipassemblyCOM(),
+ fr_trunk_X_LH_hipassemblyCOM(),
+ fr_trunk_X_RH_hipassemblyCOM(),
+ fr_trunk_X_LF_upperlegCOM(),
+ fr_trunk_X_RF_upperlegCOM(),
+ fr_trunk_X_LH_upperlegCOM(),
+ fr_trunk_X_RH_upperlegCOM(),
+ fr_trunk_X_LF_lowerlegCOM(),
+ fr_trunk_X_RF_lowerlegCOM(),
+ fr_trunk_X_LH_lowerlegCOM(),
+ fr_trunk_X_RH_lowerlegCOM(),
+ fr_LF_foot_X_fr_LF_lowerleg(),
+ fr_RF_foot_X_fr_RF_lowerleg(),
+ fr_LH_foot_X_fr_LH_lowerleg(),
+ fr_RH_foot_X_fr_RH_lowerleg(),
+ fr_trunk_X_fr_LF_foot(),
+ fr_trunk_X_fr_RF_foot(),
+ fr_trunk_X_fr_LH_foot(),
+ fr_trunk_X_fr_RH_foot(),
+ fr_LF_foot_X_fr_trunk(),
+ fr_RF_foot_X_fr_trunk(),
+ fr_LH_foot_X_fr_trunk(),
+ fr_RH_foot_X_fr_trunk(),
+ fr_trunk_X_fr_LF_HAA(),
+ fr_trunk_X_fr_RF_HAA(),
+ fr_trunk_X_fr_LH_HAA(),
+ fr_trunk_X_fr_RH_HAA(),
+ fr_trunk_X_fr_LF_HFE(),
+ fr_trunk_X_fr_RF_HFE(),
+ fr_trunk_X_fr_LH_HFE(),
+ fr_trunk_X_fr_RH_HFE(),
+ fr_trunk_X_fr_LF_KFE(),
+ fr_trunk_X_fr_RF_KFE(),
+ fr_trunk_X_fr_LH_KFE(),
+ fr_trunk_X_fr_RH_KFE(),
+ fr_LF_upperleg_X_fr_LF_hipassembly(),
+ fr_LF_hipassembly_X_fr_LF_upperleg(),
+ fr_LF_lowerleg_X_fr_LF_upperleg(),
+ fr_LF_upperleg_X_fr_LF_lowerleg(),
+ fr_RF_upperleg_X_fr_RF_hipassembly(),
+ fr_RF_hipassembly_X_fr_RF_upperleg(),
+ fr_RF_lowerleg_X_fr_RF_upperleg(),
+ fr_RF_upperleg_X_fr_RF_lowerleg(),
+ fr_LH_upperleg_X_fr_LH_hipassembly(),
+ fr_LH_hipassembly_X_fr_LH_upperleg(),
+ fr_LH_lowerleg_X_fr_LH_upperleg(),
+ fr_LH_upperleg_X_fr_LH_lowerleg(),
+ fr_RH_upperleg_X_fr_RH_hipassembly(),
+ fr_RH_hipassembly_X_fr_RH_upperleg(),
+ fr_RH_lowerleg_X_fr_RH_upperleg(),
+ fr_RH_upperleg_X_fr_RH_lowerleg()
+{
+ updateParameters();
+}
+template <typename TRAIT>
+void iit::TestHyQ::tpl::MotionTransforms<TRAIT>::updateParameters() {
+}
+
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::ForceTransforms
+ ()
+ :
+ fr_trunk_X_fr_LF_hipassembly(),
+ fr_trunk_X_fr_RF_hipassembly(),
+ fr_trunk_X_fr_LH_hipassembly(),
+ fr_trunk_X_fr_RH_hipassembly(),
+ fr_trunk_X_fr_LF_upperleg(),
+ fr_trunk_X_fr_RF_upperleg(),
+ fr_trunk_X_fr_LH_upperleg(),
+ fr_trunk_X_fr_RH_upperleg(),
+ fr_trunk_X_fr_LF_lowerleg(),
+ fr_trunk_X_fr_RF_lowerleg(),
+ fr_trunk_X_fr_LH_lowerleg(),
+ fr_trunk_X_fr_RH_lowerleg(),
+ fr_LF_hipassembly_X_fr_trunk(),
+ fr_RF_hipassembly_X_fr_trunk(),
+ fr_LH_hipassembly_X_fr_trunk(),
+ fr_RH_hipassembly_X_fr_trunk(),
+ fr_LF_upperleg_X_fr_trunk(),
+ fr_RF_upperleg_X_fr_trunk(),
+ fr_LH_upperleg_X_fr_trunk(),
+ fr_RH_upperleg_X_fr_trunk(),
+ fr_LF_lowerleg_X_fr_trunk(),
+ fr_RF_lowerleg_X_fr_trunk(),
+ fr_LH_lowerleg_X_fr_trunk(),
+ fr_RH_lowerleg_X_fr_trunk(),
+ fr_trunk_X_LF_hipassemblyCOM(),
+ fr_trunk_X_RF_hipassemblyCOM(),
+ fr_trunk_X_LH_hipassemblyCOM(),
+ fr_trunk_X_RH_hipassemblyCOM(),
+ fr_trunk_X_LF_upperlegCOM(),
+ fr_trunk_X_RF_upperlegCOM(),
+ fr_trunk_X_LH_upperlegCOM(),
+ fr_trunk_X_RH_upperlegCOM(),
+ fr_trunk_X_LF_lowerlegCOM(),
+ fr_trunk_X_RF_lowerlegCOM(),
+ fr_trunk_X_LH_lowerlegCOM(),
+ fr_trunk_X_RH_lowerlegCOM(),
+ fr_LF_foot_X_fr_LF_lowerleg(),
+ fr_RF_foot_X_fr_RF_lowerleg(),
+ fr_LH_foot_X_fr_LH_lowerleg(),
+ fr_RH_foot_X_fr_RH_lowerleg(),
+ fr_trunk_X_fr_LF_foot(),
+ fr_trunk_X_fr_RF_foot(),
+ fr_trunk_X_fr_LH_foot(),
+ fr_trunk_X_fr_RH_foot(),
+ fr_LF_foot_X_fr_trunk(),
+ fr_RF_foot_X_fr_trunk(),
+ fr_LH_foot_X_fr_trunk(),
+ fr_RH_foot_X_fr_trunk(),
+ fr_trunk_X_fr_LF_HAA(),
+ fr_trunk_X_fr_RF_HAA(),
+ fr_trunk_X_fr_LH_HAA(),
+ fr_trunk_X_fr_RH_HAA(),
+ fr_trunk_X_fr_LF_HFE(),
+ fr_trunk_X_fr_RF_HFE(),
+ fr_trunk_X_fr_LH_HFE(),
+ fr_trunk_X_fr_RH_HFE(),
+ fr_trunk_X_fr_LF_KFE(),
+ fr_trunk_X_fr_RF_KFE(),
+ fr_trunk_X_fr_LH_KFE(),
+ fr_trunk_X_fr_RH_KFE(),
+ fr_LF_upperleg_X_fr_LF_hipassembly(),
+ fr_LF_hipassembly_X_fr_LF_upperleg(),
+ fr_LF_lowerleg_X_fr_LF_upperleg(),
+ fr_LF_upperleg_X_fr_LF_lowerleg(),
+ fr_RF_upperleg_X_fr_RF_hipassembly(),
+ fr_RF_hipassembly_X_fr_RF_upperleg(),
+ fr_RF_lowerleg_X_fr_RF_upperleg(),
+ fr_RF_upperleg_X_fr_RF_lowerleg(),
+ fr_LH_upperleg_X_fr_LH_hipassembly(),
+ fr_LH_hipassembly_X_fr_LH_upperleg(),
+ fr_LH_lowerleg_X_fr_LH_upperleg(),
+ fr_LH_upperleg_X_fr_LH_lowerleg(),
+ fr_RH_upperleg_X_fr_RH_hipassembly(),
+ fr_RH_hipassembly_X_fr_RH_upperleg(),
+ fr_RH_lowerleg_X_fr_RH_upperleg(),
+ fr_RH_upperleg_X_fr_RH_lowerleg()
+{
+ updateParameters();
+}
+template <typename TRAIT>
+void iit::TestHyQ::tpl::ForceTransforms<TRAIT>::updateParameters() {
+}
+
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::HomogeneousTransforms
+ ()
+ :
+ fr_trunk_X_fr_LF_hipassembly(),
+ fr_trunk_X_fr_RF_hipassembly(),
+ fr_trunk_X_fr_LH_hipassembly(),
+ fr_trunk_X_fr_RH_hipassembly(),
+ fr_trunk_X_fr_LF_upperleg(),
+ fr_trunk_X_fr_RF_upperleg(),
+ fr_trunk_X_fr_LH_upperleg(),
+ fr_trunk_X_fr_RH_upperleg(),
+ fr_trunk_X_fr_LF_lowerleg(),
+ fr_trunk_X_fr_RF_lowerleg(),
+ fr_trunk_X_fr_LH_lowerleg(),
+ fr_trunk_X_fr_RH_lowerleg(),
+ fr_LF_hipassembly_X_fr_trunk(),
+ fr_RF_hipassembly_X_fr_trunk(),
+ fr_LH_hipassembly_X_fr_trunk(),
+ fr_RH_hipassembly_X_fr_trunk(),
+ fr_LF_upperleg_X_fr_trunk(),
+ fr_RF_upperleg_X_fr_trunk(),
+ fr_LH_upperleg_X_fr_trunk(),
+ fr_RH_upperleg_X_fr_trunk(),
+ fr_LF_lowerleg_X_fr_trunk(),
+ fr_RF_lowerleg_X_fr_trunk(),
+ fr_LH_lowerleg_X_fr_trunk(),
+ fr_RH_lowerleg_X_fr_trunk(),
+ fr_trunk_X_LF_hipassemblyCOM(),
+ fr_trunk_X_RF_hipassemblyCOM(),
+ fr_trunk_X_LH_hipassemblyCOM(),
+ fr_trunk_X_RH_hipassemblyCOM(),
+ fr_trunk_X_LF_upperlegCOM(),
+ fr_trunk_X_RF_upperlegCOM(),
+ fr_trunk_X_LH_upperlegCOM(),
+ fr_trunk_X_RH_upperlegCOM(),
+ fr_trunk_X_LF_lowerlegCOM(),
+ fr_trunk_X_RF_lowerlegCOM(),
+ fr_trunk_X_LH_lowerlegCOM(),
+ fr_trunk_X_RH_lowerlegCOM(),
+ fr_LF_foot_X_fr_LF_lowerleg(),
+ fr_RF_foot_X_fr_RF_lowerleg(),
+ fr_LH_foot_X_fr_LH_lowerleg(),
+ fr_RH_foot_X_fr_RH_lowerleg(),
+ fr_trunk_X_fr_LF_foot(),
+ fr_trunk_X_fr_RF_foot(),
+ fr_trunk_X_fr_LH_foot(),
+ fr_trunk_X_fr_RH_foot(),
+ fr_LF_foot_X_fr_trunk(),
+ fr_RF_foot_X_fr_trunk(),
+ fr_LH_foot_X_fr_trunk(),
+ fr_RH_foot_X_fr_trunk(),
+ fr_trunk_X_fr_LF_HAA(),
+ fr_trunk_X_fr_RF_HAA(),
+ fr_trunk_X_fr_LH_HAA(),
+ fr_trunk_X_fr_RH_HAA(),
+ fr_trunk_X_fr_LF_HFE(),
+ fr_trunk_X_fr_RF_HFE(),
+ fr_trunk_X_fr_LH_HFE(),
+ fr_trunk_X_fr_RH_HFE(),
+ fr_trunk_X_fr_LF_KFE(),
+ fr_trunk_X_fr_RF_KFE(),
+ fr_trunk_X_fr_LH_KFE(),
+ fr_trunk_X_fr_RH_KFE(),
+ fr_LF_upperleg_X_fr_LF_hipassembly(),
+ fr_LF_hipassembly_X_fr_LF_upperleg(),
+ fr_LF_lowerleg_X_fr_LF_upperleg(),
+ fr_LF_upperleg_X_fr_LF_lowerleg(),
+ fr_RF_upperleg_X_fr_RF_hipassembly(),
+ fr_RF_hipassembly_X_fr_RF_upperleg(),
+ fr_RF_lowerleg_X_fr_RF_upperleg(),
+ fr_RF_upperleg_X_fr_RF_lowerleg(),
+ fr_LH_upperleg_X_fr_LH_hipassembly(),
+ fr_LH_hipassembly_X_fr_LH_upperleg(),
+ fr_LH_lowerleg_X_fr_LH_upperleg(),
+ fr_LH_upperleg_X_fr_LH_lowerleg(),
+ fr_RH_upperleg_X_fr_RH_hipassembly(),
+ fr_RH_hipassembly_X_fr_RH_upperleg(),
+ fr_RH_lowerleg_X_fr_RH_upperleg(),
+ fr_RH_upperleg_X_fr_RH_lowerleg()
+{
+ updateParameters();
+}
+template <typename TRAIT>
+void iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::updateParameters() {
+}
+
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_hipassembly::Type_fr_trunk_X_fr_LF_hipassembly()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = - 1.0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = - 1.0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,2) = 0.207;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_hipassembly& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_hipassembly::update(const JState& q) {
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(1,0) = - sin__q_LF_HAA__;
+ (*this)(1,1) = - cos__q_LF_HAA__;
+ (*this)(2,0) = - cos__q_LF_HAA__;
+ (*this)(2,1) = sin__q_LF_HAA__;
+ (*this)(3,0) = (- 0.207 * cos__q_LF_HAA__);
+ (*this)(3,1) = ( 0.207 * sin__q_LF_HAA__);
+ (*this)(4,0) = ( 0.3735 * cos__q_LF_HAA__);
+ (*this)(4,1) = (- 0.3735 * sin__q_LF_HAA__);
+ (*this)(4,3) = - sin__q_LF_HAA__;
+ (*this)(4,4) = - cos__q_LF_HAA__;
+ (*this)(5,0) = (- 0.3735 * sin__q_LF_HAA__);
+ (*this)(5,1) = (- 0.3735 * cos__q_LF_HAA__);
+ (*this)(5,3) = - cos__q_LF_HAA__;
+ (*this)(5,4) = sin__q_LF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_hipassembly::Type_fr_trunk_X_fr_RF_hipassembly()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 1.0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 1.0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,2) = 0.207;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_hipassembly& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_hipassembly::update(const JState& q) {
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(1,0) = sin__q_RF_HAA__;
+ (*this)(1,1) = cos__q_RF_HAA__;
+ (*this)(2,0) = - cos__q_RF_HAA__;
+ (*this)(2,1) = sin__q_RF_HAA__;
+ (*this)(3,0) = ( 0.207 * cos__q_RF_HAA__);
+ (*this)(3,1) = (- 0.207 * sin__q_RF_HAA__);
+ (*this)(4,0) = ( 0.3735 * cos__q_RF_HAA__);
+ (*this)(4,1) = (- 0.3735 * sin__q_RF_HAA__);
+ (*this)(4,3) = sin__q_RF_HAA__;
+ (*this)(4,4) = cos__q_RF_HAA__;
+ (*this)(5,0) = ( 0.3735 * sin__q_RF_HAA__);
+ (*this)(5,1) = ( 0.3735 * cos__q_RF_HAA__);
+ (*this)(5,3) = - cos__q_RF_HAA__;
+ (*this)(5,4) = sin__q_RF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_hipassembly::Type_fr_trunk_X_fr_LH_hipassembly()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = - 1.0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = - 1.0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,2) = 0.207;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_hipassembly& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_hipassembly::update(const JState& q) {
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(1,0) = - sin__q_LH_HAA__;
+ (*this)(1,1) = - cos__q_LH_HAA__;
+ (*this)(2,0) = - cos__q_LH_HAA__;
+ (*this)(2,1) = sin__q_LH_HAA__;
+ (*this)(3,0) = (- 0.207 * cos__q_LH_HAA__);
+ (*this)(3,1) = ( 0.207 * sin__q_LH_HAA__);
+ (*this)(4,0) = (- 0.3735 * cos__q_LH_HAA__);
+ (*this)(4,1) = ( 0.3735 * sin__q_LH_HAA__);
+ (*this)(4,3) = - sin__q_LH_HAA__;
+ (*this)(4,4) = - cos__q_LH_HAA__;
+ (*this)(5,0) = ( 0.3735 * sin__q_LH_HAA__);
+ (*this)(5,1) = ( 0.3735 * cos__q_LH_HAA__);
+ (*this)(5,3) = - cos__q_LH_HAA__;
+ (*this)(5,4) = sin__q_LH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_hipassembly::Type_fr_trunk_X_fr_RH_hipassembly()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 1.0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 1.0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,2) = 0.207;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_hipassembly& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_hipassembly::update(const JState& q) {
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(1,0) = sin__q_RH_HAA__;
+ (*this)(1,1) = cos__q_RH_HAA__;
+ (*this)(2,0) = - cos__q_RH_HAA__;
+ (*this)(2,1) = sin__q_RH_HAA__;
+ (*this)(3,0) = ( 0.207 * cos__q_RH_HAA__);
+ (*this)(3,1) = (- 0.207 * sin__q_RH_HAA__);
+ (*this)(4,0) = (- 0.3735 * cos__q_RH_HAA__);
+ (*this)(4,1) = ( 0.3735 * sin__q_RH_HAA__);
+ (*this)(4,3) = sin__q_RH_HAA__;
+ (*this)(4,4) = cos__q_RH_HAA__;
+ (*this)(5,0) = (- 0.3735 * sin__q_RH_HAA__);
+ (*this)(5,1) = (- 0.3735 * cos__q_RH_HAA__);
+ (*this)(5,3) = - cos__q_RH_HAA__;
+ (*this)(5,4) = sin__q_RH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_upperleg::Type_fr_trunk_X_fr_LF_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_upperleg& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_upperleg::update(const JState& q) {
+ Scalar sin__q_LF_HFE__;
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,0) = - sin__q_LF_HFE__;
+ (*this)(0,1) = - cos__q_LF_HFE__;
+ (*this)(1,0) = (- sin__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(1,1) = ( sin__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(1,2) = cos__q_LF_HAA__;
+ (*this)(2,0) = (- cos__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(2,1) = ( cos__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(3,0) = ((- 0.207 * cos__q_LF_HAA__) * cos__q_LF_HFE__);
+ (*this)(3,1) = (( 0.207 * cos__q_LF_HAA__) * sin__q_LF_HFE__);
+ (*this)(3,2) = ( 0.08 - ( 0.207 * sin__q_LF_HAA__));
+ (*this)(3,3) = - sin__q_LF_HFE__;
+ (*this)(3,4) = - cos__q_LF_HFE__;
+ (*this)(4,0) = ((( 0.08 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.3735 * cos__q_LF_HAA__) * cos__q_LF_HFE__));
+ (*this)(4,1) = ((( 0.08 * cos__q_LF_HAA__) * cos__q_LF_HFE__) - (( 0.3735 * cos__q_LF_HAA__) * sin__q_LF_HFE__));
+ (*this)(4,2) = ( 0.3735 * sin__q_LF_HAA__);
+ (*this)(4,3) = (- sin__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(4,4) = ( sin__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(4,5) = cos__q_LF_HAA__;
+ (*this)(5,0) = ((( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * sin__q_LF_HFE__) - (( 0.3735 * sin__q_LF_HAA__) * cos__q_LF_HFE__));
+ (*this)(5,1) = ((( 0.3735 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * cos__q_LF_HFE__));
+ (*this)(5,2) = ( 0.3735 * cos__q_LF_HAA__);
+ (*this)(5,3) = (- cos__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(5,4) = ( cos__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(5,5) = - sin__q_LF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_upperleg::Type_fr_trunk_X_fr_RF_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_upperleg& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_upperleg::update(const JState& q) {
+ Scalar sin__q_RF_HFE__;
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,0) = - sin__q_RF_HFE__;
+ (*this)(0,1) = - cos__q_RF_HFE__;
+ (*this)(1,0) = ( sin__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(1,1) = (- sin__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(1,2) = cos__q_RF_HAA__;
+ (*this)(2,0) = (- cos__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(2,1) = ( cos__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(3,0) = (( 0.207 * cos__q_RF_HAA__) * cos__q_RF_HFE__);
+ (*this)(3,1) = ((- 0.207 * cos__q_RF_HAA__) * sin__q_RF_HFE__);
+ (*this)(3,2) = ( 0.08 - ( 0.207 * sin__q_RF_HAA__));
+ (*this)(3,3) = - sin__q_RF_HFE__;
+ (*this)(3,4) = - cos__q_RF_HFE__;
+ (*this)(4,0) = ((( 0.08 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.3735 * cos__q_RF_HAA__) * cos__q_RF_HFE__));
+ (*this)(4,1) = ((( 0.08 * cos__q_RF_HAA__) * cos__q_RF_HFE__) - (( 0.3735 * cos__q_RF_HAA__) * sin__q_RF_HFE__));
+ (*this)(4,2) = (- 0.3735 * sin__q_RF_HAA__);
+ (*this)(4,3) = ( sin__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(4,4) = (- sin__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(4,5) = cos__q_RF_HAA__;
+ (*this)(5,0) = (((( 0.08 * sin__q_RF_HAA__) - 0.207) * sin__q_RF_HFE__) + (( 0.3735 * sin__q_RF_HAA__) * cos__q_RF_HFE__));
+ (*this)(5,1) = (((( 0.08 * sin__q_RF_HAA__) - 0.207) * cos__q_RF_HFE__) - (( 0.3735 * sin__q_RF_HAA__) * sin__q_RF_HFE__));
+ (*this)(5,2) = ( 0.3735 * cos__q_RF_HAA__);
+ (*this)(5,3) = (- cos__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(5,4) = ( cos__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(5,5) = sin__q_RF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_upperleg::Type_fr_trunk_X_fr_LH_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_upperleg& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_upperleg::update(const JState& q) {
+ Scalar sin__q_LH_HFE__;
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,0) = - sin__q_LH_HFE__;
+ (*this)(0,1) = - cos__q_LH_HFE__;
+ (*this)(1,0) = (- sin__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(1,1) = ( sin__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(1,2) = cos__q_LH_HAA__;
+ (*this)(2,0) = (- cos__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(2,1) = ( cos__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(3,0) = ((- 0.207 * cos__q_LH_HAA__) * cos__q_LH_HFE__);
+ (*this)(3,1) = (( 0.207 * cos__q_LH_HAA__) * sin__q_LH_HFE__);
+ (*this)(3,2) = ( 0.08 - ( 0.207 * sin__q_LH_HAA__));
+ (*this)(3,3) = - sin__q_LH_HFE__;
+ (*this)(3,4) = - cos__q_LH_HFE__;
+ (*this)(4,0) = ((( 0.08 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.3735 * cos__q_LH_HAA__) * cos__q_LH_HFE__));
+ (*this)(4,1) = ((( 0.3735 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.08 * cos__q_LH_HAA__) * cos__q_LH_HFE__));
+ (*this)(4,2) = (- 0.3735 * sin__q_LH_HAA__);
+ (*this)(4,3) = (- sin__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(4,4) = ( sin__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(4,5) = cos__q_LH_HAA__;
+ (*this)(5,0) = ((( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * sin__q_LH_HFE__) + (( 0.3735 * sin__q_LH_HAA__) * cos__q_LH_HFE__));
+ (*this)(5,1) = ((( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * cos__q_LH_HFE__) - (( 0.3735 * sin__q_LH_HAA__) * sin__q_LH_HFE__));
+ (*this)(5,2) = (- 0.3735 * cos__q_LH_HAA__);
+ (*this)(5,3) = (- cos__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(5,4) = ( cos__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(5,5) = - sin__q_LH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_upperleg::Type_fr_trunk_X_fr_RH_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_upperleg& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_upperleg::update(const JState& q) {
+ Scalar sin__q_RH_HFE__;
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,0) = - sin__q_RH_HFE__;
+ (*this)(0,1) = - cos__q_RH_HFE__;
+ (*this)(1,0) = ( sin__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(1,1) = (- sin__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(1,2) = cos__q_RH_HAA__;
+ (*this)(2,0) = (- cos__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(2,1) = ( cos__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(3,0) = (( 0.207 * cos__q_RH_HAA__) * cos__q_RH_HFE__);
+ (*this)(3,1) = ((- 0.207 * cos__q_RH_HAA__) * sin__q_RH_HFE__);
+ (*this)(3,2) = ( 0.08 - ( 0.207 * sin__q_RH_HAA__));
+ (*this)(3,3) = - sin__q_RH_HFE__;
+ (*this)(3,4) = - cos__q_RH_HFE__;
+ (*this)(4,0) = ((( 0.08 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.3735 * cos__q_RH_HAA__) * cos__q_RH_HFE__));
+ (*this)(4,1) = ((( 0.3735 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.08 * cos__q_RH_HAA__) * cos__q_RH_HFE__));
+ (*this)(4,2) = ( 0.3735 * sin__q_RH_HAA__);
+ (*this)(4,3) = ( sin__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(4,4) = (- sin__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(4,5) = cos__q_RH_HAA__;
+ (*this)(5,0) = (((( 0.08 * sin__q_RH_HAA__) - 0.207) * sin__q_RH_HFE__) - (( 0.3735 * sin__q_RH_HAA__) * cos__q_RH_HFE__));
+ (*this)(5,1) = ((( 0.3735 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ((( 0.08 * sin__q_RH_HAA__) - 0.207) * cos__q_RH_HFE__));
+ (*this)(5,2) = (- 0.3735 * cos__q_RH_HAA__);
+ (*this)(5,3) = (- cos__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(5,4) = ( cos__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(5,5) = sin__q_RH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_lowerleg::Type_fr_trunk_X_fr_LF_lowerleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_lowerleg& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_lowerleg::update(const JState& q) {
+ Scalar sin__q_LF_KFE__;
+ Scalar sin__q_LF_HFE__;
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+ Scalar cos__q_LF_KFE__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_KFE__ = TRAIT::sin( q(LF_KFE));
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+ cos__q_LF_KFE__ = TRAIT::cos( q(LF_KFE));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,0) = ((- cos__q_LF_HFE__ * sin__q_LF_KFE__) - ( sin__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(0,1) = (( sin__q_LF_HFE__ * sin__q_LF_KFE__) - ( cos__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(1,0) = ((( sin__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( sin__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,1) = ((( sin__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( sin__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,2) = cos__q_LF_HAA__;
+ (*this)(2,0) = ((( cos__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( cos__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(2,1) = ((( cos__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( cos__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(3,0) = (((( 0.207 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * sin__q_LF_KFE__) - ((( 0.207 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(3,1) = (((( 0.207 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) + ((( 0.207 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(3,2) = ((( 0.35 * cos__q_LF_HFE__) - ( 0.207 * sin__q_LF_HAA__)) + 0.08);
+ (*this)(3,3) = ((- cos__q_LF_HFE__ * sin__q_LF_KFE__) - ( sin__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(3,4) = (( sin__q_LF_HFE__ * sin__q_LF_KFE__) - ( cos__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(4,0) = ((((((- 0.3735 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.08 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.35 * cos__q_LF_HAA__)) * sin__q_LF_KFE__) + (((( 0.08 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.3735 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) * cos__q_LF_KFE__));
+ (*this)(4,1) = (((((- 0.08 * cos__q_LF_HAA__) * sin__q_LF_HFE__) - (( 0.3735 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) * sin__q_LF_KFE__) + (((((- 0.3735 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.08 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.35 * cos__q_LF_HAA__)) * cos__q_LF_KFE__));
+ (*this)(4,2) = (( 0.3735 * sin__q_LF_HAA__) - (( 0.35 * sin__q_LF_HAA__) * sin__q_LF_HFE__));
+ (*this)(4,3) = ((( sin__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( sin__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(4,4) = ((( sin__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( sin__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(4,5) = cos__q_LF_HAA__;
+ (*this)(5,0) = (((((( 0.3735 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * cos__q_LF_HFE__)) - ( 0.35 * sin__q_LF_HAA__)) * sin__q_LF_KFE__) + (((( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * sin__q_LF_HFE__) - (( 0.3735 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) * cos__q_LF_KFE__));
+ (*this)(5,1) = (((((( 0.08 * sin__q_LF_HAA__) - 0.207) * sin__q_LF_HFE__) + (( 0.3735 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) * sin__q_LF_KFE__) + ((((( 0.3735 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * cos__q_LF_HFE__)) - ( 0.35 * sin__q_LF_HAA__)) * cos__q_LF_KFE__));
+ (*this)(5,2) = (( 0.3735 * cos__q_LF_HAA__) - (( 0.35 * cos__q_LF_HAA__) * sin__q_LF_HFE__));
+ (*this)(5,3) = ((( cos__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( cos__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(5,4) = ((( cos__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( cos__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(5,5) = - sin__q_LF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_lowerleg::Type_fr_trunk_X_fr_RF_lowerleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_lowerleg& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_lowerleg::update(const JState& q) {
+ Scalar sin__q_RF_KFE__;
+ Scalar sin__q_RF_HFE__;
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+ Scalar cos__q_RF_KFE__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_KFE__ = TRAIT::sin( q(RF_KFE));
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+ cos__q_RF_KFE__ = TRAIT::cos( q(RF_KFE));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,0) = ((- cos__q_RF_HFE__ * sin__q_RF_KFE__) - ( sin__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(0,1) = (( sin__q_RF_HFE__ * sin__q_RF_KFE__) - ( cos__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(1,0) = ((( sin__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(1,1) = (((- sin__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(1,2) = cos__q_RF_HAA__;
+ (*this)(2,0) = ((( cos__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__) - (( cos__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(2,1) = ((( cos__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) + (( cos__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(3,0) = (((( 0.207 * cos__q_RF_HAA__) * cos__q_RF_HFE__) * cos__q_RF_KFE__) - ((( 0.207 * cos__q_RF_HAA__) * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(3,1) = ((((- 0.207 * cos__q_RF_HAA__) * cos__q_RF_HFE__) * sin__q_RF_KFE__) - ((( 0.207 * cos__q_RF_HAA__) * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(3,2) = ((( 0.35 * cos__q_RF_HFE__) - ( 0.207 * sin__q_RF_HAA__)) + 0.08);
+ (*this)(3,3) = ((- cos__q_RF_HFE__ * sin__q_RF_KFE__) - ( sin__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(3,4) = (( sin__q_RF_HFE__ * sin__q_RF_KFE__) - ( cos__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(4,0) = ((((((- 0.3735 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.08 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.35 * cos__q_RF_HAA__)) * sin__q_RF_KFE__) + (((( 0.08 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.3735 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) * cos__q_RF_KFE__));
+ (*this)(4,1) = (((((- 0.08 * cos__q_RF_HAA__) * sin__q_RF_HFE__) - (( 0.3735 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + (((((- 0.3735 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.08 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.35 * cos__q_RF_HAA__)) * cos__q_RF_KFE__));
+ (*this)(4,2) = ((( 0.35 * sin__q_RF_HAA__) * sin__q_RF_HFE__) - ( 0.3735 * sin__q_RF_HAA__));
+ (*this)(4,3) = ((( sin__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(4,4) = (((- sin__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(4,5) = cos__q_RF_HAA__;
+ (*this)(5,0) = ((((((- 0.3735 * sin__q_RF_HAA__) * sin__q_RF_HFE__) + ((( 0.08 * sin__q_RF_HAA__) - 0.207) * cos__q_RF_HFE__)) + ( 0.35 * sin__q_RF_HAA__)) * sin__q_RF_KFE__) + ((((( 0.08 * sin__q_RF_HAA__) - 0.207) * sin__q_RF_HFE__) + (( 0.3735 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) * cos__q_RF_KFE__));
+ (*this)(5,1) = ((((( 0.207 - ( 0.08 * sin__q_RF_HAA__)) * sin__q_RF_HFE__) - (( 0.3735 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + (((((- 0.3735 * sin__q_RF_HAA__) * sin__q_RF_HFE__) + ((( 0.08 * sin__q_RF_HAA__) - 0.207) * cos__q_RF_HFE__)) + ( 0.35 * sin__q_RF_HAA__)) * cos__q_RF_KFE__));
+ (*this)(5,2) = (( 0.3735 * cos__q_RF_HAA__) - (( 0.35 * cos__q_RF_HAA__) * sin__q_RF_HFE__));
+ (*this)(5,3) = ((( cos__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__) - (( cos__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(5,4) = ((( cos__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) + (( cos__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(5,5) = sin__q_RF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_lowerleg::Type_fr_trunk_X_fr_LH_lowerleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_lowerleg& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_lowerleg::update(const JState& q) {
+ Scalar sin__q_LH_KFE__;
+ Scalar sin__q_LH_HFE__;
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+ Scalar cos__q_LH_KFE__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_KFE__ = TRAIT::sin( q(LH_KFE));
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+ cos__q_LH_KFE__ = TRAIT::cos( q(LH_KFE));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,0) = ((- cos__q_LH_HFE__ * sin__q_LH_KFE__) - ( sin__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(0,1) = (( sin__q_LH_HFE__ * sin__q_LH_KFE__) - ( cos__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(1,0) = ((( sin__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( sin__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,1) = ((( sin__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( sin__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,2) = cos__q_LH_HAA__;
+ (*this)(2,0) = ((( cos__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( cos__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(2,1) = ((( cos__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( cos__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(3,0) = (((( 0.207 * cos__q_LH_HAA__) * sin__q_LH_HFE__) * sin__q_LH_KFE__) - ((( 0.207 * cos__q_LH_HAA__) * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(3,1) = (((( 0.207 * cos__q_LH_HAA__) * cos__q_LH_HFE__) * sin__q_LH_KFE__) + ((( 0.207 * cos__q_LH_HAA__) * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(3,2) = ((( 0.35 * cos__q_LH_HFE__) - ( 0.207 * sin__q_LH_HAA__)) + 0.08);
+ (*this)(3,3) = ((- cos__q_LH_HFE__ * sin__q_LH_KFE__) - ( sin__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(3,4) = (( sin__q_LH_HFE__ * sin__q_LH_KFE__) - ( cos__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(4,0) = (((((( 0.3735 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.08 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) + ( 0.35 * cos__q_LH_HAA__)) * sin__q_LH_KFE__) + (((( 0.08 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.3735 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) * cos__q_LH_KFE__));
+ (*this)(4,1) = ((((( 0.3735 * cos__q_LH_HAA__) * cos__q_LH_HFE__) - (( 0.08 * cos__q_LH_HAA__) * sin__q_LH_HFE__)) * sin__q_LH_KFE__) + ((((( 0.3735 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.08 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) + ( 0.35 * cos__q_LH_HAA__)) * cos__q_LH_KFE__));
+ (*this)(4,2) = (((- 0.35 * sin__q_LH_HAA__) * sin__q_LH_HFE__) - ( 0.3735 * sin__q_LH_HAA__));
+ (*this)(4,3) = ((( sin__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( sin__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(4,4) = ((( sin__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( sin__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(4,5) = cos__q_LH_HAA__;
+ (*this)(5,0) = ((((((- 0.3735 * sin__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * cos__q_LH_HFE__)) - ( 0.35 * sin__q_LH_HAA__)) * sin__q_LH_KFE__) + (((( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * sin__q_LH_HFE__) + (( 0.3735 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) * cos__q_LH_KFE__));
+ (*this)(5,1) = (((((( 0.08 * sin__q_LH_HAA__) - 0.207) * sin__q_LH_HFE__) - (( 0.3735 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) * sin__q_LH_KFE__) + (((((- 0.3735 * sin__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * cos__q_LH_HFE__)) - ( 0.35 * sin__q_LH_HAA__)) * cos__q_LH_KFE__));
+ (*this)(5,2) = (((- 0.35 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - ( 0.3735 * cos__q_LH_HAA__));
+ (*this)(5,3) = ((( cos__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( cos__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(5,4) = ((( cos__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( cos__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(5,5) = - sin__q_LH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_lowerleg::Type_fr_trunk_X_fr_RH_lowerleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_lowerleg& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_lowerleg::update(const JState& q) {
+ Scalar sin__q_RH_KFE__;
+ Scalar sin__q_RH_HFE__;
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+ Scalar cos__q_RH_KFE__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_KFE__ = TRAIT::sin( q(RH_KFE));
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+ cos__q_RH_KFE__ = TRAIT::cos( q(RH_KFE));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,0) = ((- cos__q_RH_HFE__ * sin__q_RH_KFE__) - ( sin__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(0,1) = (( sin__q_RH_HFE__ * sin__q_RH_KFE__) - ( cos__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(1,0) = ((( sin__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(1,1) = (((- sin__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(1,2) = cos__q_RH_HAA__;
+ (*this)(2,0) = ((( cos__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__) - (( cos__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(2,1) = ((( cos__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) + (( cos__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(3,0) = (((( 0.207 * cos__q_RH_HAA__) * cos__q_RH_HFE__) * cos__q_RH_KFE__) - ((( 0.207 * cos__q_RH_HAA__) * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(3,1) = ((((- 0.207 * cos__q_RH_HAA__) * cos__q_RH_HFE__) * sin__q_RH_KFE__) - ((( 0.207 * cos__q_RH_HAA__) * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(3,2) = ((( 0.35 * cos__q_RH_HFE__) - ( 0.207 * sin__q_RH_HAA__)) + 0.08);
+ (*this)(3,3) = ((- cos__q_RH_HFE__ * sin__q_RH_KFE__) - ( sin__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(3,4) = (( sin__q_RH_HFE__ * sin__q_RH_KFE__) - ( cos__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(4,0) = (((((( 0.3735 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.08 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.35 * cos__q_RH_HAA__)) * sin__q_RH_KFE__) + (((( 0.08 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.3735 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) * cos__q_RH_KFE__));
+ (*this)(4,1) = ((((( 0.3735 * cos__q_RH_HAA__) * cos__q_RH_HFE__) - (( 0.08 * cos__q_RH_HAA__) * sin__q_RH_HFE__)) * sin__q_RH_KFE__) + ((((( 0.3735 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.08 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.35 * cos__q_RH_HAA__)) * cos__q_RH_KFE__));
+ (*this)(4,2) = ((( 0.35 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ( 0.3735 * sin__q_RH_HAA__));
+ (*this)(4,3) = ((( sin__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(4,4) = (((- sin__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(4,5) = cos__q_RH_HAA__;
+ (*this)(5,0) = (((((( 0.3735 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ((( 0.08 * sin__q_RH_HAA__) - 0.207) * cos__q_RH_HFE__)) + ( 0.35 * sin__q_RH_HAA__)) * sin__q_RH_KFE__) + ((((( 0.08 * sin__q_RH_HAA__) - 0.207) * sin__q_RH_HFE__) - (( 0.3735 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) * cos__q_RH_KFE__));
+ (*this)(5,1) = ((((( 0.207 - ( 0.08 * sin__q_RH_HAA__)) * sin__q_RH_HFE__) + (( 0.3735 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) * sin__q_RH_KFE__) + ((((( 0.3735 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ((( 0.08 * sin__q_RH_HAA__) - 0.207) * cos__q_RH_HFE__)) + ( 0.35 * sin__q_RH_HAA__)) * cos__q_RH_KFE__));
+ (*this)(5,2) = (((- 0.35 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - ( 0.3735 * cos__q_RH_HAA__));
+ (*this)(5,3) = ((( cos__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__) - (( cos__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(5,4) = ((( cos__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) + (( cos__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(5,5) = sin__q_RH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LF_hipassembly_X_fr_trunk::Type_fr_LF_hipassembly_X_fr_trunk()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = - 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,3) = 0;
+ (*this)(4,3) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0.207;
+ (*this)(5,3) = - 1.0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LF_hipassembly_X_fr_trunk& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LF_hipassembly_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,1) = - sin__q_LF_HAA__;
+ (*this)(0,2) = - cos__q_LF_HAA__;
+ (*this)(1,1) = - cos__q_LF_HAA__;
+ (*this)(1,2) = sin__q_LF_HAA__;
+ (*this)(3,0) = (- 0.207 * cos__q_LF_HAA__);
+ (*this)(3,1) = ( 0.3735 * cos__q_LF_HAA__);
+ (*this)(3,2) = (- 0.3735 * sin__q_LF_HAA__);
+ (*this)(3,4) = - sin__q_LF_HAA__;
+ (*this)(3,5) = - cos__q_LF_HAA__;
+ (*this)(4,0) = ( 0.207 * sin__q_LF_HAA__);
+ (*this)(4,1) = (- 0.3735 * sin__q_LF_HAA__);
+ (*this)(4,2) = (- 0.3735 * cos__q_LF_HAA__);
+ (*this)(4,4) = - cos__q_LF_HAA__;
+ (*this)(4,5) = sin__q_LF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RF_hipassembly_X_fr_trunk::Type_fr_RF_hipassembly_X_fr_trunk()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,3) = 0;
+ (*this)(4,3) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0.207;
+ (*this)(5,3) = 1.0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RF_hipassembly_X_fr_trunk& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RF_hipassembly_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,1) = sin__q_RF_HAA__;
+ (*this)(0,2) = - cos__q_RF_HAA__;
+ (*this)(1,1) = cos__q_RF_HAA__;
+ (*this)(1,2) = sin__q_RF_HAA__;
+ (*this)(3,0) = ( 0.207 * cos__q_RF_HAA__);
+ (*this)(3,1) = ( 0.3735 * cos__q_RF_HAA__);
+ (*this)(3,2) = ( 0.3735 * sin__q_RF_HAA__);
+ (*this)(3,4) = sin__q_RF_HAA__;
+ (*this)(3,5) = - cos__q_RF_HAA__;
+ (*this)(4,0) = (- 0.207 * sin__q_RF_HAA__);
+ (*this)(4,1) = (- 0.3735 * sin__q_RF_HAA__);
+ (*this)(4,2) = ( 0.3735 * cos__q_RF_HAA__);
+ (*this)(4,4) = cos__q_RF_HAA__;
+ (*this)(4,5) = sin__q_RF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LH_hipassembly_X_fr_trunk::Type_fr_LH_hipassembly_X_fr_trunk()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = - 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,3) = 0;
+ (*this)(4,3) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0.207;
+ (*this)(5,3) = - 1.0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LH_hipassembly_X_fr_trunk& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LH_hipassembly_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,1) = - sin__q_LH_HAA__;
+ (*this)(0,2) = - cos__q_LH_HAA__;
+ (*this)(1,1) = - cos__q_LH_HAA__;
+ (*this)(1,2) = sin__q_LH_HAA__;
+ (*this)(3,0) = (- 0.207 * cos__q_LH_HAA__);
+ (*this)(3,1) = (- 0.3735 * cos__q_LH_HAA__);
+ (*this)(3,2) = ( 0.3735 * sin__q_LH_HAA__);
+ (*this)(3,4) = - sin__q_LH_HAA__;
+ (*this)(3,5) = - cos__q_LH_HAA__;
+ (*this)(4,0) = ( 0.207 * sin__q_LH_HAA__);
+ (*this)(4,1) = ( 0.3735 * sin__q_LH_HAA__);
+ (*this)(4,2) = ( 0.3735 * cos__q_LH_HAA__);
+ (*this)(4,4) = - cos__q_LH_HAA__;
+ (*this)(4,5) = sin__q_LH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RH_hipassembly_X_fr_trunk::Type_fr_RH_hipassembly_X_fr_trunk()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,3) = 0;
+ (*this)(4,3) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0.207;
+ (*this)(5,3) = 1.0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RH_hipassembly_X_fr_trunk& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RH_hipassembly_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,1) = sin__q_RH_HAA__;
+ (*this)(0,2) = - cos__q_RH_HAA__;
+ (*this)(1,1) = cos__q_RH_HAA__;
+ (*this)(1,2) = sin__q_RH_HAA__;
+ (*this)(3,0) = ( 0.207 * cos__q_RH_HAA__);
+ (*this)(3,1) = (- 0.3735 * cos__q_RH_HAA__);
+ (*this)(3,2) = (- 0.3735 * sin__q_RH_HAA__);
+ (*this)(3,4) = sin__q_RH_HAA__;
+ (*this)(3,5) = - cos__q_RH_HAA__;
+ (*this)(4,0) = (- 0.207 * sin__q_RH_HAA__);
+ (*this)(4,1) = ( 0.3735 * sin__q_RH_HAA__);
+ (*this)(4,2) = (- 0.3735 * cos__q_RH_HAA__);
+ (*this)(4,4) = cos__q_RH_HAA__;
+ (*this)(4,5) = sin__q_RH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LF_upperleg_X_fr_trunk::Type_fr_LF_upperleg_X_fr_trunk()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(5,3) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LF_upperleg_X_fr_trunk& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LF_upperleg_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_LF_HFE__;
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,0) = - sin__q_LF_HFE__;
+ (*this)(0,1) = (- sin__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(0,2) = (- cos__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(1,0) = - cos__q_LF_HFE__;
+ (*this)(1,1) = ( sin__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(1,2) = ( cos__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(2,1) = cos__q_LF_HAA__;
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(3,0) = ((- 0.207 * cos__q_LF_HAA__) * cos__q_LF_HFE__);
+ (*this)(3,1) = ((( 0.08 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.3735 * cos__q_LF_HAA__) * cos__q_LF_HFE__));
+ (*this)(3,2) = ((( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * sin__q_LF_HFE__) - (( 0.3735 * sin__q_LF_HAA__) * cos__q_LF_HFE__));
+ (*this)(3,3) = - sin__q_LF_HFE__;
+ (*this)(3,4) = (- sin__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(3,5) = (- cos__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(4,0) = (( 0.207 * cos__q_LF_HAA__) * sin__q_LF_HFE__);
+ (*this)(4,1) = ((( 0.08 * cos__q_LF_HAA__) * cos__q_LF_HFE__) - (( 0.3735 * cos__q_LF_HAA__) * sin__q_LF_HFE__));
+ (*this)(4,2) = ((( 0.3735 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * cos__q_LF_HFE__));
+ (*this)(4,3) = - cos__q_LF_HFE__;
+ (*this)(4,4) = ( sin__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(4,5) = ( cos__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(5,0) = ( 0.08 - ( 0.207 * sin__q_LF_HAA__));
+ (*this)(5,1) = ( 0.3735 * sin__q_LF_HAA__);
+ (*this)(5,2) = ( 0.3735 * cos__q_LF_HAA__);
+ (*this)(5,4) = cos__q_LF_HAA__;
+ (*this)(5,5) = - sin__q_LF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RF_upperleg_X_fr_trunk::Type_fr_RF_upperleg_X_fr_trunk()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(5,3) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RF_upperleg_X_fr_trunk& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RF_upperleg_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_RF_HFE__;
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,0) = - sin__q_RF_HFE__;
+ (*this)(0,1) = ( sin__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(0,2) = (- cos__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(1,0) = - cos__q_RF_HFE__;
+ (*this)(1,1) = (- sin__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(1,2) = ( cos__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(2,1) = cos__q_RF_HAA__;
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(3,0) = (( 0.207 * cos__q_RF_HAA__) * cos__q_RF_HFE__);
+ (*this)(3,1) = ((( 0.08 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.3735 * cos__q_RF_HAA__) * cos__q_RF_HFE__));
+ (*this)(3,2) = (((( 0.08 * sin__q_RF_HAA__) - 0.207) * sin__q_RF_HFE__) + (( 0.3735 * sin__q_RF_HAA__) * cos__q_RF_HFE__));
+ (*this)(3,3) = - sin__q_RF_HFE__;
+ (*this)(3,4) = ( sin__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(3,5) = (- cos__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(4,0) = ((- 0.207 * cos__q_RF_HAA__) * sin__q_RF_HFE__);
+ (*this)(4,1) = ((( 0.08 * cos__q_RF_HAA__) * cos__q_RF_HFE__) - (( 0.3735 * cos__q_RF_HAA__) * sin__q_RF_HFE__));
+ (*this)(4,2) = (((( 0.08 * sin__q_RF_HAA__) - 0.207) * cos__q_RF_HFE__) - (( 0.3735 * sin__q_RF_HAA__) * sin__q_RF_HFE__));
+ (*this)(4,3) = - cos__q_RF_HFE__;
+ (*this)(4,4) = (- sin__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(4,5) = ( cos__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(5,0) = ( 0.08 - ( 0.207 * sin__q_RF_HAA__));
+ (*this)(5,1) = (- 0.3735 * sin__q_RF_HAA__);
+ (*this)(5,2) = ( 0.3735 * cos__q_RF_HAA__);
+ (*this)(5,4) = cos__q_RF_HAA__;
+ (*this)(5,5) = sin__q_RF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LH_upperleg_X_fr_trunk::Type_fr_LH_upperleg_X_fr_trunk()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(5,3) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LH_upperleg_X_fr_trunk& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LH_upperleg_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_LH_HFE__;
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,0) = - sin__q_LH_HFE__;
+ (*this)(0,1) = (- sin__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(0,2) = (- cos__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(1,0) = - cos__q_LH_HFE__;
+ (*this)(1,1) = ( sin__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(1,2) = ( cos__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(2,1) = cos__q_LH_HAA__;
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(3,0) = ((- 0.207 * cos__q_LH_HAA__) * cos__q_LH_HFE__);
+ (*this)(3,1) = ((( 0.08 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.3735 * cos__q_LH_HAA__) * cos__q_LH_HFE__));
+ (*this)(3,2) = ((( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * sin__q_LH_HFE__) + (( 0.3735 * sin__q_LH_HAA__) * cos__q_LH_HFE__));
+ (*this)(3,3) = - sin__q_LH_HFE__;
+ (*this)(3,4) = (- sin__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(3,5) = (- cos__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(4,0) = (( 0.207 * cos__q_LH_HAA__) * sin__q_LH_HFE__);
+ (*this)(4,1) = ((( 0.3735 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.08 * cos__q_LH_HAA__) * cos__q_LH_HFE__));
+ (*this)(4,2) = ((( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * cos__q_LH_HFE__) - (( 0.3735 * sin__q_LH_HAA__) * sin__q_LH_HFE__));
+ (*this)(4,3) = - cos__q_LH_HFE__;
+ (*this)(4,4) = ( sin__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(4,5) = ( cos__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(5,0) = ( 0.08 - ( 0.207 * sin__q_LH_HAA__));
+ (*this)(5,1) = (- 0.3735 * sin__q_LH_HAA__);
+ (*this)(5,2) = (- 0.3735 * cos__q_LH_HAA__);
+ (*this)(5,4) = cos__q_LH_HAA__;
+ (*this)(5,5) = - sin__q_LH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RH_upperleg_X_fr_trunk::Type_fr_RH_upperleg_X_fr_trunk()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(5,3) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RH_upperleg_X_fr_trunk& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RH_upperleg_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_RH_HFE__;
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,0) = - sin__q_RH_HFE__;
+ (*this)(0,1) = ( sin__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(0,2) = (- cos__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(1,0) = - cos__q_RH_HFE__;
+ (*this)(1,1) = (- sin__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(1,2) = ( cos__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(2,1) = cos__q_RH_HAA__;
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(3,0) = (( 0.207 * cos__q_RH_HAA__) * cos__q_RH_HFE__);
+ (*this)(3,1) = ((( 0.08 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.3735 * cos__q_RH_HAA__) * cos__q_RH_HFE__));
+ (*this)(3,2) = (((( 0.08 * sin__q_RH_HAA__) - 0.207) * sin__q_RH_HFE__) - (( 0.3735 * sin__q_RH_HAA__) * cos__q_RH_HFE__));
+ (*this)(3,3) = - sin__q_RH_HFE__;
+ (*this)(3,4) = ( sin__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(3,5) = (- cos__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(4,0) = ((- 0.207 * cos__q_RH_HAA__) * sin__q_RH_HFE__);
+ (*this)(4,1) = ((( 0.3735 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.08 * cos__q_RH_HAA__) * cos__q_RH_HFE__));
+ (*this)(4,2) = ((( 0.3735 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ((( 0.08 * sin__q_RH_HAA__) - 0.207) * cos__q_RH_HFE__));
+ (*this)(4,3) = - cos__q_RH_HFE__;
+ (*this)(4,4) = (- sin__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(4,5) = ( cos__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(5,0) = ( 0.08 - ( 0.207 * sin__q_RH_HAA__));
+ (*this)(5,1) = ( 0.3735 * sin__q_RH_HAA__);
+ (*this)(5,2) = (- 0.3735 * cos__q_RH_HAA__);
+ (*this)(5,4) = cos__q_RH_HAA__;
+ (*this)(5,5) = sin__q_RH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LF_lowerleg_X_fr_trunk::Type_fr_LF_lowerleg_X_fr_trunk()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(5,3) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LF_lowerleg_X_fr_trunk& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LF_lowerleg_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_LF_KFE__;
+ Scalar sin__q_LF_HFE__;
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+ Scalar cos__q_LF_KFE__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_KFE__ = TRAIT::sin( q(LF_KFE));
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+ cos__q_LF_KFE__ = TRAIT::cos( q(LF_KFE));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,0) = ((- cos__q_LF_HFE__ * sin__q_LF_KFE__) - ( sin__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(0,1) = ((( sin__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( sin__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(0,2) = ((( cos__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( cos__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,0) = (( sin__q_LF_HFE__ * sin__q_LF_KFE__) - ( cos__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(1,1) = ((( sin__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( sin__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,2) = ((( cos__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( cos__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(2,1) = cos__q_LF_HAA__;
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(3,0) = (((( 0.207 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * sin__q_LF_KFE__) - ((( 0.207 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(3,1) = ((((((- 0.3735 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.08 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.35 * cos__q_LF_HAA__)) * sin__q_LF_KFE__) + (((( 0.08 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.3735 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) * cos__q_LF_KFE__));
+ (*this)(3,2) = (((((( 0.3735 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * cos__q_LF_HFE__)) - ( 0.35 * sin__q_LF_HAA__)) * sin__q_LF_KFE__) + (((( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * sin__q_LF_HFE__) - (( 0.3735 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) * cos__q_LF_KFE__));
+ (*this)(3,3) = ((- cos__q_LF_HFE__ * sin__q_LF_KFE__) - ( sin__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(3,4) = ((( sin__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( sin__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(3,5) = ((( cos__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( cos__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(4,0) = (((( 0.207 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) + ((( 0.207 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(4,1) = (((((- 0.08 * cos__q_LF_HAA__) * sin__q_LF_HFE__) - (( 0.3735 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) * sin__q_LF_KFE__) + (((((- 0.3735 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.08 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.35 * cos__q_LF_HAA__)) * cos__q_LF_KFE__));
+ (*this)(4,2) = (((((( 0.08 * sin__q_LF_HAA__) - 0.207) * sin__q_LF_HFE__) + (( 0.3735 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) * sin__q_LF_KFE__) + ((((( 0.3735 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * cos__q_LF_HFE__)) - ( 0.35 * sin__q_LF_HAA__)) * cos__q_LF_KFE__));
+ (*this)(4,3) = (( sin__q_LF_HFE__ * sin__q_LF_KFE__) - ( cos__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(4,4) = ((( sin__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( sin__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(4,5) = ((( cos__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( cos__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(5,0) = ((( 0.35 * cos__q_LF_HFE__) - ( 0.207 * sin__q_LF_HAA__)) + 0.08);
+ (*this)(5,1) = (( 0.3735 * sin__q_LF_HAA__) - (( 0.35 * sin__q_LF_HAA__) * sin__q_LF_HFE__));
+ (*this)(5,2) = (( 0.3735 * cos__q_LF_HAA__) - (( 0.35 * cos__q_LF_HAA__) * sin__q_LF_HFE__));
+ (*this)(5,4) = cos__q_LF_HAA__;
+ (*this)(5,5) = - sin__q_LF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RF_lowerleg_X_fr_trunk::Type_fr_RF_lowerleg_X_fr_trunk()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(5,3) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RF_lowerleg_X_fr_trunk& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RF_lowerleg_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_RF_KFE__;
+ Scalar sin__q_RF_HFE__;
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+ Scalar cos__q_RF_KFE__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_KFE__ = TRAIT::sin( q(RF_KFE));
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+ cos__q_RF_KFE__ = TRAIT::cos( q(RF_KFE));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,0) = ((- cos__q_RF_HFE__ * sin__q_RF_KFE__) - ( sin__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(0,1) = ((( sin__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(0,2) = ((( cos__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__) - (( cos__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(1,0) = (( sin__q_RF_HFE__ * sin__q_RF_KFE__) - ( cos__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(1,1) = (((- sin__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(1,2) = ((( cos__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) + (( cos__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(2,1) = cos__q_RF_HAA__;
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(3,0) = (((( 0.207 * cos__q_RF_HAA__) * cos__q_RF_HFE__) * cos__q_RF_KFE__) - ((( 0.207 * cos__q_RF_HAA__) * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(3,1) = ((((((- 0.3735 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.08 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.35 * cos__q_RF_HAA__)) * sin__q_RF_KFE__) + (((( 0.08 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.3735 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) * cos__q_RF_KFE__));
+ (*this)(3,2) = ((((((- 0.3735 * sin__q_RF_HAA__) * sin__q_RF_HFE__) + ((( 0.08 * sin__q_RF_HAA__) - 0.207) * cos__q_RF_HFE__)) + ( 0.35 * sin__q_RF_HAA__)) * sin__q_RF_KFE__) + ((((( 0.08 * sin__q_RF_HAA__) - 0.207) * sin__q_RF_HFE__) + (( 0.3735 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) * cos__q_RF_KFE__));
+ (*this)(3,3) = ((- cos__q_RF_HFE__ * sin__q_RF_KFE__) - ( sin__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(3,4) = ((( sin__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(3,5) = ((( cos__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__) - (( cos__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(4,0) = ((((- 0.207 * cos__q_RF_HAA__) * cos__q_RF_HFE__) * sin__q_RF_KFE__) - ((( 0.207 * cos__q_RF_HAA__) * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(4,1) = (((((- 0.08 * cos__q_RF_HAA__) * sin__q_RF_HFE__) - (( 0.3735 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + (((((- 0.3735 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.08 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.35 * cos__q_RF_HAA__)) * cos__q_RF_KFE__));
+ (*this)(4,2) = ((((( 0.207 - ( 0.08 * sin__q_RF_HAA__)) * sin__q_RF_HFE__) - (( 0.3735 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + (((((- 0.3735 * sin__q_RF_HAA__) * sin__q_RF_HFE__) + ((( 0.08 * sin__q_RF_HAA__) - 0.207) * cos__q_RF_HFE__)) + ( 0.35 * sin__q_RF_HAA__)) * cos__q_RF_KFE__));
+ (*this)(4,3) = (( sin__q_RF_HFE__ * sin__q_RF_KFE__) - ( cos__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(4,4) = (((- sin__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(4,5) = ((( cos__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) + (( cos__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(5,0) = ((( 0.35 * cos__q_RF_HFE__) - ( 0.207 * sin__q_RF_HAA__)) + 0.08);
+ (*this)(5,1) = ((( 0.35 * sin__q_RF_HAA__) * sin__q_RF_HFE__) - ( 0.3735 * sin__q_RF_HAA__));
+ (*this)(5,2) = (( 0.3735 * cos__q_RF_HAA__) - (( 0.35 * cos__q_RF_HAA__) * sin__q_RF_HFE__));
+ (*this)(5,4) = cos__q_RF_HAA__;
+ (*this)(5,5) = sin__q_RF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LH_lowerleg_X_fr_trunk::Type_fr_LH_lowerleg_X_fr_trunk()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(5,3) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LH_lowerleg_X_fr_trunk& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LH_lowerleg_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_LH_KFE__;
+ Scalar sin__q_LH_HFE__;
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+ Scalar cos__q_LH_KFE__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_KFE__ = TRAIT::sin( q(LH_KFE));
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+ cos__q_LH_KFE__ = TRAIT::cos( q(LH_KFE));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,0) = ((- cos__q_LH_HFE__ * sin__q_LH_KFE__) - ( sin__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(0,1) = ((( sin__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( sin__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(0,2) = ((( cos__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( cos__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,0) = (( sin__q_LH_HFE__ * sin__q_LH_KFE__) - ( cos__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(1,1) = ((( sin__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( sin__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,2) = ((( cos__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( cos__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(2,1) = cos__q_LH_HAA__;
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(3,0) = (((( 0.207 * cos__q_LH_HAA__) * sin__q_LH_HFE__) * sin__q_LH_KFE__) - ((( 0.207 * cos__q_LH_HAA__) * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(3,1) = (((((( 0.3735 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.08 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) + ( 0.35 * cos__q_LH_HAA__)) * sin__q_LH_KFE__) + (((( 0.08 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.3735 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) * cos__q_LH_KFE__));
+ (*this)(3,2) = ((((((- 0.3735 * sin__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * cos__q_LH_HFE__)) - ( 0.35 * sin__q_LH_HAA__)) * sin__q_LH_KFE__) + (((( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * sin__q_LH_HFE__) + (( 0.3735 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) * cos__q_LH_KFE__));
+ (*this)(3,3) = ((- cos__q_LH_HFE__ * sin__q_LH_KFE__) - ( sin__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(3,4) = ((( sin__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( sin__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(3,5) = ((( cos__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( cos__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(4,0) = (((( 0.207 * cos__q_LH_HAA__) * cos__q_LH_HFE__) * sin__q_LH_KFE__) + ((( 0.207 * cos__q_LH_HAA__) * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(4,1) = ((((( 0.3735 * cos__q_LH_HAA__) * cos__q_LH_HFE__) - (( 0.08 * cos__q_LH_HAA__) * sin__q_LH_HFE__)) * sin__q_LH_KFE__) + ((((( 0.3735 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.08 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) + ( 0.35 * cos__q_LH_HAA__)) * cos__q_LH_KFE__));
+ (*this)(4,2) = (((((( 0.08 * sin__q_LH_HAA__) - 0.207) * sin__q_LH_HFE__) - (( 0.3735 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) * sin__q_LH_KFE__) + (((((- 0.3735 * sin__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * cos__q_LH_HFE__)) - ( 0.35 * sin__q_LH_HAA__)) * cos__q_LH_KFE__));
+ (*this)(4,3) = (( sin__q_LH_HFE__ * sin__q_LH_KFE__) - ( cos__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(4,4) = ((( sin__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( sin__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(4,5) = ((( cos__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( cos__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(5,0) = ((( 0.35 * cos__q_LH_HFE__) - ( 0.207 * sin__q_LH_HAA__)) + 0.08);
+ (*this)(5,1) = (((- 0.35 * sin__q_LH_HAA__) * sin__q_LH_HFE__) - ( 0.3735 * sin__q_LH_HAA__));
+ (*this)(5,2) = (((- 0.35 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - ( 0.3735 * cos__q_LH_HAA__));
+ (*this)(5,4) = cos__q_LH_HAA__;
+ (*this)(5,5) = - sin__q_LH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RH_lowerleg_X_fr_trunk::Type_fr_RH_lowerleg_X_fr_trunk()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(5,3) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RH_lowerleg_X_fr_trunk& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RH_lowerleg_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_RH_KFE__;
+ Scalar sin__q_RH_HFE__;
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+ Scalar cos__q_RH_KFE__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_KFE__ = TRAIT::sin( q(RH_KFE));
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+ cos__q_RH_KFE__ = TRAIT::cos( q(RH_KFE));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,0) = ((- cos__q_RH_HFE__ * sin__q_RH_KFE__) - ( sin__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(0,1) = ((( sin__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(0,2) = ((( cos__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__) - (( cos__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(1,0) = (( sin__q_RH_HFE__ * sin__q_RH_KFE__) - ( cos__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(1,1) = (((- sin__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(1,2) = ((( cos__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) + (( cos__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(2,1) = cos__q_RH_HAA__;
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(3,0) = (((( 0.207 * cos__q_RH_HAA__) * cos__q_RH_HFE__) * cos__q_RH_KFE__) - ((( 0.207 * cos__q_RH_HAA__) * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(3,1) = (((((( 0.3735 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.08 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.35 * cos__q_RH_HAA__)) * sin__q_RH_KFE__) + (((( 0.08 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.3735 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) * cos__q_RH_KFE__));
+ (*this)(3,2) = (((((( 0.3735 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ((( 0.08 * sin__q_RH_HAA__) - 0.207) * cos__q_RH_HFE__)) + ( 0.35 * sin__q_RH_HAA__)) * sin__q_RH_KFE__) + ((((( 0.08 * sin__q_RH_HAA__) - 0.207) * sin__q_RH_HFE__) - (( 0.3735 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) * cos__q_RH_KFE__));
+ (*this)(3,3) = ((- cos__q_RH_HFE__ * sin__q_RH_KFE__) - ( sin__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(3,4) = ((( sin__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(3,5) = ((( cos__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__) - (( cos__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(4,0) = ((((- 0.207 * cos__q_RH_HAA__) * cos__q_RH_HFE__) * sin__q_RH_KFE__) - ((( 0.207 * cos__q_RH_HAA__) * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(4,1) = ((((( 0.3735 * cos__q_RH_HAA__) * cos__q_RH_HFE__) - (( 0.08 * cos__q_RH_HAA__) * sin__q_RH_HFE__)) * sin__q_RH_KFE__) + ((((( 0.3735 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.08 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.35 * cos__q_RH_HAA__)) * cos__q_RH_KFE__));
+ (*this)(4,2) = ((((( 0.207 - ( 0.08 * sin__q_RH_HAA__)) * sin__q_RH_HFE__) + (( 0.3735 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) * sin__q_RH_KFE__) + ((((( 0.3735 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ((( 0.08 * sin__q_RH_HAA__) - 0.207) * cos__q_RH_HFE__)) + ( 0.35 * sin__q_RH_HAA__)) * cos__q_RH_KFE__));
+ (*this)(4,3) = (( sin__q_RH_HFE__ * sin__q_RH_KFE__) - ( cos__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(4,4) = (((- sin__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(4,5) = ((( cos__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) + (( cos__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(5,0) = ((( 0.35 * cos__q_RH_HFE__) - ( 0.207 * sin__q_RH_HAA__)) + 0.08);
+ (*this)(5,1) = ((( 0.35 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ( 0.3735 * sin__q_RH_HAA__));
+ (*this)(5,2) = (((- 0.35 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - ( 0.3735 * cos__q_RH_HAA__));
+ (*this)(5,4) = cos__q_RH_HAA__;
+ (*this)(5,5) = sin__q_RH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_LF_hipassemblyCOM::Type_fr_trunk_X_LF_hipassemblyCOM()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = - 1.0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = - 1.0;
+ (*this)(4,5) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_LF_hipassemblyCOM& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_LF_hipassemblyCOM::update(const JState& q) {
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(1,0) = - sin__q_LF_HAA__;
+ (*this)(1,1) = - cos__q_LF_HAA__;
+ (*this)(2,0) = - cos__q_LF_HAA__;
+ (*this)(2,1) = sin__q_LF_HAA__;
+ (*this)(3,0) = (- 0.207 * cos__q_LF_HAA__);
+ (*this)(3,1) = (( 0.207 * sin__q_LF_HAA__) - 0.043);
+ (*this)(4,0) = ( 0.2045 * cos__q_LF_HAA__);
+ (*this)(4,1) = (- 0.2045 * sin__q_LF_HAA__);
+ (*this)(4,2) = ( 0.043 * cos__q_LF_HAA__);
+ (*this)(4,3) = - sin__q_LF_HAA__;
+ (*this)(4,4) = - cos__q_LF_HAA__;
+ (*this)(5,0) = (- 0.2045 * sin__q_LF_HAA__);
+ (*this)(5,1) = (- 0.2045 * cos__q_LF_HAA__);
+ (*this)(5,2) = ( 0.207 - ( 0.043 * sin__q_LF_HAA__));
+ (*this)(5,3) = - cos__q_LF_HAA__;
+ (*this)(5,4) = sin__q_LF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_RF_hipassemblyCOM::Type_fr_trunk_X_RF_hipassemblyCOM()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 1.0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 1.0;
+ (*this)(4,5) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_RF_hipassemblyCOM& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_RF_hipassemblyCOM::update(const JState& q) {
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(1,0) = sin__q_RF_HAA__;
+ (*this)(1,1) = cos__q_RF_HAA__;
+ (*this)(2,0) = - cos__q_RF_HAA__;
+ (*this)(2,1) = sin__q_RF_HAA__;
+ (*this)(3,0) = ( 0.207 * cos__q_RF_HAA__);
+ (*this)(3,1) = ( 0.043 - ( 0.207 * sin__q_RF_HAA__));
+ (*this)(4,0) = ( 0.2045 * cos__q_RF_HAA__);
+ (*this)(4,1) = (- 0.2045 * sin__q_RF_HAA__);
+ (*this)(4,2) = (- 0.043 * cos__q_RF_HAA__);
+ (*this)(4,3) = sin__q_RF_HAA__;
+ (*this)(4,4) = cos__q_RF_HAA__;
+ (*this)(5,0) = ( 0.2045 * sin__q_RF_HAA__);
+ (*this)(5,1) = ( 0.2045 * cos__q_RF_HAA__);
+ (*this)(5,2) = ( 0.207 - ( 0.043 * sin__q_RF_HAA__));
+ (*this)(5,3) = - cos__q_RF_HAA__;
+ (*this)(5,4) = sin__q_RF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_LH_hipassemblyCOM::Type_fr_trunk_X_LH_hipassemblyCOM()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = - 1.0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = - 1.0;
+ (*this)(4,5) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_LH_hipassemblyCOM& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_LH_hipassemblyCOM::update(const JState& q) {
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(1,0) = - sin__q_LH_HAA__;
+ (*this)(1,1) = - cos__q_LH_HAA__;
+ (*this)(2,0) = - cos__q_LH_HAA__;
+ (*this)(2,1) = sin__q_LH_HAA__;
+ (*this)(3,0) = (- 0.207 * cos__q_LH_HAA__);
+ (*this)(3,1) = (( 0.207 * sin__q_LH_HAA__) - 0.043);
+ (*this)(4,0) = (- 0.2045 * cos__q_LH_HAA__);
+ (*this)(4,1) = ( 0.2045 * sin__q_LH_HAA__);
+ (*this)(4,2) = ( 0.043 * cos__q_LH_HAA__);
+ (*this)(4,3) = - sin__q_LH_HAA__;
+ (*this)(4,4) = - cos__q_LH_HAA__;
+ (*this)(5,0) = ( 0.2045 * sin__q_LH_HAA__);
+ (*this)(5,1) = ( 0.2045 * cos__q_LH_HAA__);
+ (*this)(5,2) = ( 0.207 - ( 0.043 * sin__q_LH_HAA__));
+ (*this)(5,3) = - cos__q_LH_HAA__;
+ (*this)(5,4) = sin__q_LH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_RH_hipassemblyCOM::Type_fr_trunk_X_RH_hipassemblyCOM()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 1.0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 1.0;
+ (*this)(4,5) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_RH_hipassemblyCOM& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_RH_hipassemblyCOM::update(const JState& q) {
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(1,0) = sin__q_RH_HAA__;
+ (*this)(1,1) = cos__q_RH_HAA__;
+ (*this)(2,0) = - cos__q_RH_HAA__;
+ (*this)(2,1) = sin__q_RH_HAA__;
+ (*this)(3,0) = ( 0.207 * cos__q_RH_HAA__);
+ (*this)(3,1) = ( 0.043 - ( 0.207 * sin__q_RH_HAA__));
+ (*this)(4,0) = (- 0.2045 * cos__q_RH_HAA__);
+ (*this)(4,1) = ( 0.2045 * sin__q_RH_HAA__);
+ (*this)(4,2) = (- 0.043 * cos__q_RH_HAA__);
+ (*this)(4,3) = sin__q_RH_HAA__;
+ (*this)(4,4) = cos__q_RH_HAA__;
+ (*this)(5,0) = (- 0.2045 * sin__q_RH_HAA__);
+ (*this)(5,1) = (- 0.2045 * cos__q_RH_HAA__);
+ (*this)(5,2) = ( 0.207 - ( 0.043 * sin__q_RH_HAA__));
+ (*this)(5,3) = - cos__q_RH_HAA__;
+ (*this)(5,4) = sin__q_RH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_LF_upperlegCOM::Type_fr_trunk_X_LF_upperlegCOM()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_LF_upperlegCOM& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_LF_upperlegCOM::update(const JState& q) {
+ Scalar sin__q_LF_HFE__;
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,0) = - sin__q_LF_HFE__;
+ (*this)(0,1) = - cos__q_LF_HFE__;
+ (*this)(1,0) = (- sin__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(1,1) = ( sin__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(1,2) = cos__q_LF_HAA__;
+ (*this)(2,0) = (- cos__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(2,1) = ( cos__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(3,0) = ((- 0.207 * cos__q_LF_HAA__) * cos__q_LF_HFE__);
+ (*this)(3,1) = (( 0.207 * cos__q_LF_HAA__) * sin__q_LF_HFE__);
+ (*this)(3,2) = (((( 0.026 * sin__q_LF_HFE__) + ( 0.151 * cos__q_LF_HFE__)) - ( 0.207 * sin__q_LF_HAA__)) + 0.08);
+ (*this)(3,3) = - sin__q_LF_HFE__;
+ (*this)(3,4) = - cos__q_LF_HFE__;
+ (*this)(4,0) = (((( 0.08 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.3735 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.026 * cos__q_LF_HAA__));
+ (*this)(4,1) = ((((- 0.3735 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.08 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.151 * cos__q_LF_HAA__));
+ (*this)(4,2) = ((((- 0.151 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.026 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.3735 * sin__q_LF_HAA__));
+ (*this)(4,3) = (- sin__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(4,4) = ( sin__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(4,5) = cos__q_LF_HAA__;
+ (*this)(5,0) = (((( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * sin__q_LF_HFE__) - (( 0.3735 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) - ( 0.026 * sin__q_LF_HAA__));
+ (*this)(5,1) = (((( 0.3735 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * cos__q_LF_HFE__)) - ( 0.151 * sin__q_LF_HAA__));
+ (*this)(5,2) = ((((- 0.151 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.026 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.3735 * cos__q_LF_HAA__));
+ (*this)(5,3) = (- cos__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(5,4) = ( cos__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(5,5) = - sin__q_LF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_RF_upperlegCOM::Type_fr_trunk_X_RF_upperlegCOM()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_RF_upperlegCOM& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_RF_upperlegCOM::update(const JState& q) {
+ Scalar sin__q_RF_HFE__;
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,0) = - sin__q_RF_HFE__;
+ (*this)(0,1) = - cos__q_RF_HFE__;
+ (*this)(1,0) = ( sin__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(1,1) = (- sin__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(1,2) = cos__q_RF_HAA__;
+ (*this)(2,0) = (- cos__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(2,1) = ( cos__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(3,0) = (( 0.207 * cos__q_RF_HAA__) * cos__q_RF_HFE__);
+ (*this)(3,1) = ((- 0.207 * cos__q_RF_HAA__) * sin__q_RF_HFE__);
+ (*this)(3,2) = (((( 0.026 * sin__q_RF_HFE__) + ( 0.151 * cos__q_RF_HFE__)) - ( 0.207 * sin__q_RF_HAA__)) + 0.08);
+ (*this)(3,3) = - sin__q_RF_HFE__;
+ (*this)(3,4) = - cos__q_RF_HFE__;
+ (*this)(4,0) = (((( 0.08 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.3735 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.026 * cos__q_RF_HAA__));
+ (*this)(4,1) = ((((- 0.3735 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.08 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.151 * cos__q_RF_HAA__));
+ (*this)(4,2) = (((( 0.151 * sin__q_RF_HAA__) * sin__q_RF_HFE__) - (( 0.026 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) - ( 0.3735 * sin__q_RF_HAA__));
+ (*this)(4,3) = ( sin__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(4,4) = (- sin__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(4,5) = cos__q_RF_HAA__;
+ (*this)(5,0) = ((((( 0.08 * sin__q_RF_HAA__) - 0.207) * sin__q_RF_HFE__) + (( 0.3735 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.026 * sin__q_RF_HAA__));
+ (*this)(5,1) = ((((- 0.3735 * sin__q_RF_HAA__) * sin__q_RF_HFE__) + ((( 0.08 * sin__q_RF_HAA__) - 0.207) * cos__q_RF_HFE__)) + ( 0.151 * sin__q_RF_HAA__));
+ (*this)(5,2) = ((((- 0.151 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.026 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.3735 * cos__q_RF_HAA__));
+ (*this)(5,3) = (- cos__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(5,4) = ( cos__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(5,5) = sin__q_RF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_LH_upperlegCOM::Type_fr_trunk_X_LH_upperlegCOM()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_LH_upperlegCOM& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_LH_upperlegCOM::update(const JState& q) {
+ Scalar sin__q_LH_HFE__;
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,0) = - sin__q_LH_HFE__;
+ (*this)(0,1) = - cos__q_LH_HFE__;
+ (*this)(1,0) = (- sin__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(1,1) = ( sin__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(1,2) = cos__q_LH_HAA__;
+ (*this)(2,0) = (- cos__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(2,1) = ( cos__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(3,0) = ((- 0.207 * cos__q_LH_HAA__) * cos__q_LH_HFE__);
+ (*this)(3,1) = (( 0.207 * cos__q_LH_HAA__) * sin__q_LH_HFE__);
+ (*this)(3,2) = ((((- 0.026 * sin__q_LH_HFE__) + ( 0.151 * cos__q_LH_HFE__)) - ( 0.207 * sin__q_LH_HAA__)) + 0.08);
+ (*this)(3,3) = - sin__q_LH_HFE__;
+ (*this)(3,4) = - cos__q_LH_HFE__;
+ (*this)(4,0) = (((( 0.08 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.3735 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) - ( 0.026 * cos__q_LH_HAA__));
+ (*this)(4,1) = (((( 0.3735 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.08 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) + ( 0.151 * cos__q_LH_HAA__));
+ (*this)(4,2) = ((((- 0.151 * sin__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.026 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) - ( 0.3735 * sin__q_LH_HAA__));
+ (*this)(4,3) = (- sin__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(4,4) = ( sin__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(4,5) = cos__q_LH_HAA__;
+ (*this)(5,0) = (((( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * sin__q_LH_HFE__) + (( 0.3735 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) + ( 0.026 * sin__q_LH_HAA__));
+ (*this)(5,1) = ((((- 0.3735 * sin__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * cos__q_LH_HFE__)) - ( 0.151 * sin__q_LH_HAA__));
+ (*this)(5,2) = ((((- 0.151 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.026 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) - ( 0.3735 * cos__q_LH_HAA__));
+ (*this)(5,3) = (- cos__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(5,4) = ( cos__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(5,5) = - sin__q_LH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_RH_upperlegCOM::Type_fr_trunk_X_RH_upperlegCOM()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_RH_upperlegCOM& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_RH_upperlegCOM::update(const JState& q) {
+ Scalar sin__q_RH_HFE__;
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,0) = - sin__q_RH_HFE__;
+ (*this)(0,1) = - cos__q_RH_HFE__;
+ (*this)(1,0) = ( sin__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(1,1) = (- sin__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(1,2) = cos__q_RH_HAA__;
+ (*this)(2,0) = (- cos__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(2,1) = ( cos__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(3,0) = (( 0.207 * cos__q_RH_HAA__) * cos__q_RH_HFE__);
+ (*this)(3,1) = ((- 0.207 * cos__q_RH_HAA__) * sin__q_RH_HFE__);
+ (*this)(3,2) = ((((- 0.026 * sin__q_RH_HFE__) + ( 0.151 * cos__q_RH_HFE__)) - ( 0.207 * sin__q_RH_HAA__)) + 0.08);
+ (*this)(3,3) = - sin__q_RH_HFE__;
+ (*this)(3,4) = - cos__q_RH_HFE__;
+ (*this)(4,0) = (((( 0.08 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.3735 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) - ( 0.026 * cos__q_RH_HAA__));
+ (*this)(4,1) = (((( 0.3735 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.08 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.151 * cos__q_RH_HAA__));
+ (*this)(4,2) = (((( 0.151 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.026 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.3735 * sin__q_RH_HAA__));
+ (*this)(4,3) = ( sin__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(4,4) = (- sin__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(4,5) = cos__q_RH_HAA__;
+ (*this)(5,0) = ((((( 0.08 * sin__q_RH_HAA__) - 0.207) * sin__q_RH_HFE__) - (( 0.3735 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) - ( 0.026 * sin__q_RH_HAA__));
+ (*this)(5,1) = (((( 0.3735 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ((( 0.08 * sin__q_RH_HAA__) - 0.207) * cos__q_RH_HFE__)) + ( 0.151 * sin__q_RH_HAA__));
+ (*this)(5,2) = ((((- 0.151 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.026 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) - ( 0.3735 * cos__q_RH_HAA__));
+ (*this)(5,3) = (- cos__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(5,4) = ( cos__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(5,5) = sin__q_RH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_LF_lowerlegCOM::Type_fr_trunk_X_LF_lowerlegCOM()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_LF_lowerlegCOM& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_LF_lowerlegCOM::update(const JState& q) {
+ Scalar sin__q_LF_KFE__;
+ Scalar sin__q_LF_HFE__;
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+ Scalar cos__q_LF_KFE__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_KFE__ = TRAIT::sin( q(LF_KFE));
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+ cos__q_LF_KFE__ = TRAIT::cos( q(LF_KFE));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,0) = ((- cos__q_LF_HFE__ * sin__q_LF_KFE__) - ( sin__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(0,1) = (( sin__q_LF_HFE__ * sin__q_LF_KFE__) - ( cos__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(1,0) = ((( sin__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( sin__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,1) = ((( sin__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( sin__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,2) = cos__q_LF_HAA__;
+ (*this)(2,0) = ((( cos__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( cos__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(2,1) = ((( cos__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( cos__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(3,0) = (((( 0.207 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * sin__q_LF_KFE__) - ((( 0.207 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(3,1) = (((( 0.207 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) + ((( 0.207 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(3,2) = ((((((- 0.125 * sin__q_LF_HFE__) * sin__q_LF_KFE__) + (( 0.125 * cos__q_LF_HFE__) * cos__q_LF_KFE__)) + ( 0.35 * cos__q_LF_HFE__)) - ( 0.207 * sin__q_LF_HAA__)) + 0.08);
+ (*this)(3,3) = ((- cos__q_LF_HFE__ * sin__q_LF_KFE__) - ( sin__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(3,4) = (( sin__q_LF_HFE__ * sin__q_LF_KFE__) - ( cos__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(4,0) = ((((((- 0.3735 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.08 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.35 * cos__q_LF_HAA__)) * sin__q_LF_KFE__) + (((( 0.08 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.3735 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) * cos__q_LF_KFE__));
+ (*this)(4,1) = ((((((- 0.08 * cos__q_LF_HAA__) * sin__q_LF_HFE__) - (( 0.3735 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) * sin__q_LF_KFE__) + (((((- 0.3735 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.08 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.35 * cos__q_LF_HAA__)) * cos__q_LF_KFE__)) + ( 0.125 * cos__q_LF_HAA__));
+ (*this)(4,2) = ((((((- 0.125 * sin__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) - ((( 0.125 * sin__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__)) - (( 0.35 * sin__q_LF_HAA__) * sin__q_LF_HFE__)) + ( 0.3735 * sin__q_LF_HAA__));
+ (*this)(4,3) = ((( sin__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( sin__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(4,4) = ((( sin__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( sin__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(4,5) = cos__q_LF_HAA__;
+ (*this)(5,0) = (((((( 0.3735 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * cos__q_LF_HFE__)) - ( 0.35 * sin__q_LF_HAA__)) * sin__q_LF_KFE__) + (((( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * sin__q_LF_HFE__) - (( 0.3735 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) * cos__q_LF_KFE__));
+ (*this)(5,1) = ((((((( 0.08 * sin__q_LF_HAA__) - 0.207) * sin__q_LF_HFE__) + (( 0.3735 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) * sin__q_LF_KFE__) + ((((( 0.3735 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * cos__q_LF_HFE__)) - ( 0.35 * sin__q_LF_HAA__)) * cos__q_LF_KFE__)) - ( 0.125 * sin__q_LF_HAA__));
+ (*this)(5,2) = ((((((- 0.125 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) - ((( 0.125 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__)) - (( 0.35 * cos__q_LF_HAA__) * sin__q_LF_HFE__)) + ( 0.3735 * cos__q_LF_HAA__));
+ (*this)(5,3) = ((( cos__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( cos__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(5,4) = ((( cos__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( cos__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(5,5) = - sin__q_LF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_RF_lowerlegCOM::Type_fr_trunk_X_RF_lowerlegCOM()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_RF_lowerlegCOM& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_RF_lowerlegCOM::update(const JState& q) {
+ Scalar sin__q_RF_KFE__;
+ Scalar sin__q_RF_HFE__;
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+ Scalar cos__q_RF_KFE__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_KFE__ = TRAIT::sin( q(RF_KFE));
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+ cos__q_RF_KFE__ = TRAIT::cos( q(RF_KFE));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,0) = ((- cos__q_RF_HFE__ * sin__q_RF_KFE__) - ( sin__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(0,1) = (( sin__q_RF_HFE__ * sin__q_RF_KFE__) - ( cos__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(1,0) = ((( sin__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(1,1) = (((- sin__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(1,2) = cos__q_RF_HAA__;
+ (*this)(2,0) = ((( cos__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__) - (( cos__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(2,1) = ((( cos__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) + (( cos__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(3,0) = (((( 0.207 * cos__q_RF_HAA__) * cos__q_RF_HFE__) * cos__q_RF_KFE__) - ((( 0.207 * cos__q_RF_HAA__) * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(3,1) = ((((- 0.207 * cos__q_RF_HAA__) * cos__q_RF_HFE__) * sin__q_RF_KFE__) - ((( 0.207 * cos__q_RF_HAA__) * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(3,2) = (((((((- 0.125 * sin__q_RF_HFE__) - ( 0.001 * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + ((( 0.125 * cos__q_RF_HFE__) - ( 0.001 * sin__q_RF_HFE__)) * cos__q_RF_KFE__)) + ( 0.35 * cos__q_RF_HFE__)) - ( 0.207 * sin__q_RF_HAA__)) + 0.08);
+ (*this)(3,3) = ((- cos__q_RF_HFE__ * sin__q_RF_KFE__) - ( sin__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(3,4) = (( sin__q_RF_HFE__ * sin__q_RF_KFE__) - ( cos__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(4,0) = (((((((- 0.3735 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.08 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.35 * cos__q_RF_HAA__)) * sin__q_RF_KFE__) + (((( 0.08 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.3735 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) * cos__q_RF_KFE__)) - ( 0.001 * cos__q_RF_HAA__));
+ (*this)(4,1) = ((((((- 0.08 * cos__q_RF_HAA__) * sin__q_RF_HFE__) - (( 0.3735 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + (((((- 0.3735 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.08 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.35 * cos__q_RF_HAA__)) * cos__q_RF_KFE__)) + ( 0.125 * cos__q_RF_HAA__));
+ (*this)(4,2) = ((((((( 0.125 * sin__q_RF_HAA__) * cos__q_RF_HFE__) - (( 0.001 * sin__q_RF_HAA__) * sin__q_RF_HFE__)) * sin__q_RF_KFE__) + (((( 0.125 * sin__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.001 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) * cos__q_RF_KFE__)) + (( 0.35 * sin__q_RF_HAA__) * sin__q_RF_HFE__)) - ( 0.3735 * sin__q_RF_HAA__));
+ (*this)(4,3) = ((( sin__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(4,4) = (((- sin__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(4,5) = cos__q_RF_HAA__;
+ (*this)(5,0) = (((((((- 0.3735 * sin__q_RF_HAA__) * sin__q_RF_HFE__) + ((( 0.08 * sin__q_RF_HAA__) - 0.207) * cos__q_RF_HFE__)) + ( 0.35 * sin__q_RF_HAA__)) * sin__q_RF_KFE__) + ((((( 0.08 * sin__q_RF_HAA__) - 0.207) * sin__q_RF_HFE__) + (( 0.3735 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) * cos__q_RF_KFE__)) - ( 0.001 * sin__q_RF_HAA__));
+ (*this)(5,1) = (((((( 0.207 - ( 0.08 * sin__q_RF_HAA__)) * sin__q_RF_HFE__) - (( 0.3735 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + (((((- 0.3735 * sin__q_RF_HAA__) * sin__q_RF_HFE__) + ((( 0.08 * sin__q_RF_HAA__) - 0.207) * cos__q_RF_HFE__)) + ( 0.35 * sin__q_RF_HAA__)) * cos__q_RF_KFE__)) + ( 0.125 * sin__q_RF_HAA__));
+ (*this)(5,2) = ((((((( 0.001 * cos__q_RF_HAA__) * sin__q_RF_HFE__) - (( 0.125 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + ((((- 0.125 * cos__q_RF_HAA__) * sin__q_RF_HFE__) - (( 0.001 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) * cos__q_RF_KFE__)) - (( 0.35 * cos__q_RF_HAA__) * sin__q_RF_HFE__)) + ( 0.3735 * cos__q_RF_HAA__));
+ (*this)(5,3) = ((( cos__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__) - (( cos__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(5,4) = ((( cos__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) + (( cos__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(5,5) = sin__q_RF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_LH_lowerlegCOM::Type_fr_trunk_X_LH_lowerlegCOM()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_LH_lowerlegCOM& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_LH_lowerlegCOM::update(const JState& q) {
+ Scalar sin__q_LH_KFE__;
+ Scalar sin__q_LH_HFE__;
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+ Scalar cos__q_LH_KFE__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_KFE__ = TRAIT::sin( q(LH_KFE));
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+ cos__q_LH_KFE__ = TRAIT::cos( q(LH_KFE));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,0) = ((- cos__q_LH_HFE__ * sin__q_LH_KFE__) - ( sin__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(0,1) = (( sin__q_LH_HFE__ * sin__q_LH_KFE__) - ( cos__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(1,0) = ((( sin__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( sin__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,1) = ((( sin__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( sin__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,2) = cos__q_LH_HAA__;
+ (*this)(2,0) = ((( cos__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( cos__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(2,1) = ((( cos__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( cos__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(3,0) = (((( 0.207 * cos__q_LH_HAA__) * sin__q_LH_HFE__) * sin__q_LH_KFE__) - ((( 0.207 * cos__q_LH_HAA__) * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(3,1) = (((( 0.207 * cos__q_LH_HAA__) * cos__q_LH_HFE__) * sin__q_LH_KFE__) + ((( 0.207 * cos__q_LH_HAA__) * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(3,2) = ((((((( 0.001 * cos__q_LH_HFE__) - ( 0.125 * sin__q_LH_HFE__)) * sin__q_LH_KFE__) + ((( 0.001 * sin__q_LH_HFE__) + ( 0.125 * cos__q_LH_HFE__)) * cos__q_LH_KFE__)) + ( 0.35 * cos__q_LH_HFE__)) - ( 0.207 * sin__q_LH_HAA__)) + 0.08);
+ (*this)(3,3) = ((- cos__q_LH_HFE__ * sin__q_LH_KFE__) - ( sin__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(3,4) = (( sin__q_LH_HFE__ * sin__q_LH_KFE__) - ( cos__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(4,0) = ((((((( 0.3735 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.08 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) + ( 0.35 * cos__q_LH_HAA__)) * sin__q_LH_KFE__) + (((( 0.08 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.3735 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) * cos__q_LH_KFE__)) + ( 0.001 * cos__q_LH_HAA__));
+ (*this)(4,1) = (((((( 0.3735 * cos__q_LH_HAA__) * cos__q_LH_HFE__) - (( 0.08 * cos__q_LH_HAA__) * sin__q_LH_HFE__)) * sin__q_LH_KFE__) + ((((( 0.3735 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.08 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) + ( 0.35 * cos__q_LH_HAA__)) * cos__q_LH_KFE__)) + ( 0.125 * cos__q_LH_HAA__));
+ (*this)(4,2) = (((((((- 0.001 * sin__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.125 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) * sin__q_LH_KFE__) + (((( 0.001 * sin__q_LH_HAA__) * cos__q_LH_HFE__) - (( 0.125 * sin__q_LH_HAA__) * sin__q_LH_HFE__)) * cos__q_LH_KFE__)) - (( 0.35 * sin__q_LH_HAA__) * sin__q_LH_HFE__)) - ( 0.3735 * sin__q_LH_HAA__));
+ (*this)(4,3) = ((( sin__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( sin__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(4,4) = ((( sin__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( sin__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(4,5) = cos__q_LH_HAA__;
+ (*this)(5,0) = (((((((- 0.3735 * sin__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * cos__q_LH_HFE__)) - ( 0.35 * sin__q_LH_HAA__)) * sin__q_LH_KFE__) + (((( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * sin__q_LH_HFE__) + (( 0.3735 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) * cos__q_LH_KFE__)) - ( 0.001 * sin__q_LH_HAA__));
+ (*this)(5,1) = ((((((( 0.08 * sin__q_LH_HAA__) - 0.207) * sin__q_LH_HFE__) - (( 0.3735 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) * sin__q_LH_KFE__) + (((((- 0.3735 * sin__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * cos__q_LH_HFE__)) - ( 0.35 * sin__q_LH_HAA__)) * cos__q_LH_KFE__)) - ( 0.125 * sin__q_LH_HAA__));
+ (*this)(5,2) = (((((((- 0.001 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.125 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) * sin__q_LH_KFE__) + (((( 0.001 * cos__q_LH_HAA__) * cos__q_LH_HFE__) - (( 0.125 * cos__q_LH_HAA__) * sin__q_LH_HFE__)) * cos__q_LH_KFE__)) - (( 0.35 * cos__q_LH_HAA__) * sin__q_LH_HFE__)) - ( 0.3735 * cos__q_LH_HAA__));
+ (*this)(5,3) = ((( cos__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( cos__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(5,4) = ((( cos__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( cos__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(5,5) = - sin__q_LH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_RH_lowerlegCOM::Type_fr_trunk_X_RH_lowerlegCOM()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_RH_lowerlegCOM& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_RH_lowerlegCOM::update(const JState& q) {
+ Scalar sin__q_RH_KFE__;
+ Scalar sin__q_RH_HFE__;
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+ Scalar cos__q_RH_KFE__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_KFE__ = TRAIT::sin( q(RH_KFE));
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+ cos__q_RH_KFE__ = TRAIT::cos( q(RH_KFE));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,0) = ((- cos__q_RH_HFE__ * sin__q_RH_KFE__) - ( sin__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(0,1) = (( sin__q_RH_HFE__ * sin__q_RH_KFE__) - ( cos__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(1,0) = ((( sin__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(1,1) = (((- sin__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(1,2) = cos__q_RH_HAA__;
+ (*this)(2,0) = ((( cos__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__) - (( cos__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(2,1) = ((( cos__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) + (( cos__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(3,0) = (((( 0.207 * cos__q_RH_HAA__) * cos__q_RH_HFE__) * cos__q_RH_KFE__) - ((( 0.207 * cos__q_RH_HAA__) * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(3,1) = ((((- 0.207 * cos__q_RH_HAA__) * cos__q_RH_HFE__) * sin__q_RH_KFE__) - ((( 0.207 * cos__q_RH_HAA__) * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(3,2) = ((((((( 0.001 * cos__q_RH_HFE__) - ( 0.125 * sin__q_RH_HFE__)) * sin__q_RH_KFE__) + ((( 0.001 * sin__q_RH_HFE__) + ( 0.125 * cos__q_RH_HFE__)) * cos__q_RH_KFE__)) + ( 0.35 * cos__q_RH_HFE__)) - ( 0.207 * sin__q_RH_HAA__)) + 0.08);
+ (*this)(3,3) = ((- cos__q_RH_HFE__ * sin__q_RH_KFE__) - ( sin__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(3,4) = (( sin__q_RH_HFE__ * sin__q_RH_KFE__) - ( cos__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(4,0) = ((((((( 0.3735 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.08 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.35 * cos__q_RH_HAA__)) * sin__q_RH_KFE__) + (((( 0.08 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.3735 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) * cos__q_RH_KFE__)) + ( 0.001 * cos__q_RH_HAA__));
+ (*this)(4,1) = (((((( 0.3735 * cos__q_RH_HAA__) * cos__q_RH_HFE__) - (( 0.08 * cos__q_RH_HAA__) * sin__q_RH_HFE__)) * sin__q_RH_KFE__) + ((((( 0.3735 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.08 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.35 * cos__q_RH_HAA__)) * cos__q_RH_KFE__)) + ( 0.125 * cos__q_RH_HAA__));
+ (*this)(4,2) = ((((((( 0.001 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.125 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) * sin__q_RH_KFE__) + (((( 0.125 * sin__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.001 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) * cos__q_RH_KFE__)) + (( 0.35 * sin__q_RH_HAA__) * sin__q_RH_HFE__)) + ( 0.3735 * sin__q_RH_HAA__));
+ (*this)(4,3) = ((( sin__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(4,4) = (((- sin__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(4,5) = cos__q_RH_HAA__;
+ (*this)(5,0) = ((((((( 0.3735 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ((( 0.08 * sin__q_RH_HAA__) - 0.207) * cos__q_RH_HFE__)) + ( 0.35 * sin__q_RH_HAA__)) * sin__q_RH_KFE__) + ((((( 0.08 * sin__q_RH_HAA__) - 0.207) * sin__q_RH_HFE__) - (( 0.3735 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) * cos__q_RH_KFE__)) + ( 0.001 * sin__q_RH_HAA__));
+ (*this)(5,1) = (((((( 0.207 - ( 0.08 * sin__q_RH_HAA__)) * sin__q_RH_HFE__) + (( 0.3735 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) * sin__q_RH_KFE__) + ((((( 0.3735 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ((( 0.08 * sin__q_RH_HAA__) - 0.207) * cos__q_RH_HFE__)) + ( 0.35 * sin__q_RH_HAA__)) * cos__q_RH_KFE__)) + ( 0.125 * sin__q_RH_HAA__));
+ (*this)(5,2) = (((((((- 0.001 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.125 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) * sin__q_RH_KFE__) + (((( 0.001 * cos__q_RH_HAA__) * cos__q_RH_HFE__) - (( 0.125 * cos__q_RH_HAA__) * sin__q_RH_HFE__)) * cos__q_RH_KFE__)) - (( 0.35 * cos__q_RH_HAA__) * sin__q_RH_HFE__)) - ( 0.3735 * cos__q_RH_HAA__));
+ (*this)(5,3) = ((( cos__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__) - (( cos__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(5,4) = ((( cos__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) + (( cos__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(5,5) = sin__q_RH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LF_foot_X_fr_LF_lowerleg::Type_fr_LF_foot_X_fr_LF_lowerleg()
+{
+ (*this)(0,0) = 1;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 1;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0.33;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = 1;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = - 0.33;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LF_foot_X_fr_LF_lowerleg& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LF_foot_X_fr_LF_lowerleg::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RF_foot_X_fr_RF_lowerleg::Type_fr_RF_foot_X_fr_RF_lowerleg()
+{
+ (*this)(0,0) = 1;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 1;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0.33;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = 1;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = - 0.33;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RF_foot_X_fr_RF_lowerleg& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RF_foot_X_fr_RF_lowerleg::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LH_foot_X_fr_LH_lowerleg::Type_fr_LH_foot_X_fr_LH_lowerleg()
+{
+ (*this)(0,0) = 1;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 1;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0.33;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = 1;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = - 0.33;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LH_foot_X_fr_LH_lowerleg& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LH_foot_X_fr_LH_lowerleg::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RH_foot_X_fr_RH_lowerleg::Type_fr_RH_foot_X_fr_RH_lowerleg()
+{
+ (*this)(0,0) = 1;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 1;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0.33;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = 1;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = - 0.33;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RH_foot_X_fr_RH_lowerleg& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RH_foot_X_fr_RH_lowerleg::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_foot::Type_fr_trunk_X_fr_LF_foot()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_foot& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_foot::update(const JState& q) {
+ Scalar sin__q_LF_KFE__;
+ Scalar sin__q_LF_HFE__;
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+ Scalar cos__q_LF_KFE__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_KFE__ = TRAIT::sin( q(LF_KFE));
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+ cos__q_LF_KFE__ = TRAIT::cos( q(LF_KFE));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,0) = ((- cos__q_LF_HFE__ * sin__q_LF_KFE__) - ( sin__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(0,1) = (( sin__q_LF_HFE__ * sin__q_LF_KFE__) - ( cos__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(1,0) = ((( sin__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( sin__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,1) = ((( sin__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( sin__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,2) = cos__q_LF_HAA__;
+ (*this)(2,0) = ((( cos__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( cos__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(2,1) = ((( cos__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( cos__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(3,0) = (((( 0.207 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * sin__q_LF_KFE__) - ((( 0.207 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(3,1) = (((( 0.207 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) + ((( 0.207 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(3,2) = ((((((- 0.33 * sin__q_LF_HFE__) * sin__q_LF_KFE__) + (( 0.33 * cos__q_LF_HFE__) * cos__q_LF_KFE__)) + ( 0.35 * cos__q_LF_HFE__)) - ( 0.207 * sin__q_LF_HAA__)) + 0.08);
+ (*this)(3,3) = ((- cos__q_LF_HFE__ * sin__q_LF_KFE__) - ( sin__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(3,4) = (( sin__q_LF_HFE__ * sin__q_LF_KFE__) - ( cos__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(4,0) = ((((((- 0.3735 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.08 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.35 * cos__q_LF_HAA__)) * sin__q_LF_KFE__) + (((( 0.08 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.3735 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) * cos__q_LF_KFE__));
+ (*this)(4,1) = ((((((- 0.08 * cos__q_LF_HAA__) * sin__q_LF_HFE__) - (( 0.3735 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) * sin__q_LF_KFE__) + (((((- 0.3735 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.08 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.35 * cos__q_LF_HAA__)) * cos__q_LF_KFE__)) + ( 0.33 * cos__q_LF_HAA__));
+ (*this)(4,2) = ((((((- 0.33 * sin__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) - ((( 0.33 * sin__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__)) - (( 0.35 * sin__q_LF_HAA__) * sin__q_LF_HFE__)) + ( 0.3735 * sin__q_LF_HAA__));
+ (*this)(4,3) = ((( sin__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( sin__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(4,4) = ((( sin__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( sin__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(4,5) = cos__q_LF_HAA__;
+ (*this)(5,0) = (((((( 0.3735 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * cos__q_LF_HFE__)) - ( 0.35 * sin__q_LF_HAA__)) * sin__q_LF_KFE__) + (((( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * sin__q_LF_HFE__) - (( 0.3735 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) * cos__q_LF_KFE__));
+ (*this)(5,1) = ((((((( 0.08 * sin__q_LF_HAA__) - 0.207) * sin__q_LF_HFE__) + (( 0.3735 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) * sin__q_LF_KFE__) + ((((( 0.3735 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * cos__q_LF_HFE__)) - ( 0.35 * sin__q_LF_HAA__)) * cos__q_LF_KFE__)) - ( 0.33 * sin__q_LF_HAA__));
+ (*this)(5,2) = ((((((- 0.33 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) - ((( 0.33 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__)) - (( 0.35 * cos__q_LF_HAA__) * sin__q_LF_HFE__)) + ( 0.3735 * cos__q_LF_HAA__));
+ (*this)(5,3) = ((( cos__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( cos__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(5,4) = ((( cos__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( cos__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(5,5) = - sin__q_LF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_foot::Type_fr_trunk_X_fr_RF_foot()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_foot& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_foot::update(const JState& q) {
+ Scalar sin__q_RF_KFE__;
+ Scalar sin__q_RF_HFE__;
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+ Scalar cos__q_RF_KFE__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_KFE__ = TRAIT::sin( q(RF_KFE));
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+ cos__q_RF_KFE__ = TRAIT::cos( q(RF_KFE));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,0) = ((- cos__q_RF_HFE__ * sin__q_RF_KFE__) - ( sin__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(0,1) = (( sin__q_RF_HFE__ * sin__q_RF_KFE__) - ( cos__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(1,0) = ((( sin__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(1,1) = (((- sin__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(1,2) = cos__q_RF_HAA__;
+ (*this)(2,0) = ((( cos__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__) - (( cos__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(2,1) = ((( cos__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) + (( cos__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(3,0) = (((( 0.207 * cos__q_RF_HAA__) * cos__q_RF_HFE__) * cos__q_RF_KFE__) - ((( 0.207 * cos__q_RF_HAA__) * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(3,1) = ((((- 0.207 * cos__q_RF_HAA__) * cos__q_RF_HFE__) * sin__q_RF_KFE__) - ((( 0.207 * cos__q_RF_HAA__) * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(3,2) = ((((((- 0.33 * sin__q_RF_HFE__) * sin__q_RF_KFE__) + (( 0.33 * cos__q_RF_HFE__) * cos__q_RF_KFE__)) + ( 0.35 * cos__q_RF_HFE__)) - ( 0.207 * sin__q_RF_HAA__)) + 0.08);
+ (*this)(3,3) = ((- cos__q_RF_HFE__ * sin__q_RF_KFE__) - ( sin__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(3,4) = (( sin__q_RF_HFE__ * sin__q_RF_KFE__) - ( cos__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(4,0) = ((((((- 0.3735 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.08 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.35 * cos__q_RF_HAA__)) * sin__q_RF_KFE__) + (((( 0.08 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.3735 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) * cos__q_RF_KFE__));
+ (*this)(4,1) = ((((((- 0.08 * cos__q_RF_HAA__) * sin__q_RF_HFE__) - (( 0.3735 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + (((((- 0.3735 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.08 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.35 * cos__q_RF_HAA__)) * cos__q_RF_KFE__)) + ( 0.33 * cos__q_RF_HAA__));
+ (*this)(4,2) = (((((( 0.33 * sin__q_RF_HAA__) * cos__q_RF_HFE__) * sin__q_RF_KFE__) + ((( 0.33 * sin__q_RF_HAA__) * sin__q_RF_HFE__) * cos__q_RF_KFE__)) + (( 0.35 * sin__q_RF_HAA__) * sin__q_RF_HFE__)) - ( 0.3735 * sin__q_RF_HAA__));
+ (*this)(4,3) = ((( sin__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(4,4) = (((- sin__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(4,5) = cos__q_RF_HAA__;
+ (*this)(5,0) = ((((((- 0.3735 * sin__q_RF_HAA__) * sin__q_RF_HFE__) + ((( 0.08 * sin__q_RF_HAA__) - 0.207) * cos__q_RF_HFE__)) + ( 0.35 * sin__q_RF_HAA__)) * sin__q_RF_KFE__) + ((((( 0.08 * sin__q_RF_HAA__) - 0.207) * sin__q_RF_HFE__) + (( 0.3735 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) * cos__q_RF_KFE__));
+ (*this)(5,1) = (((((( 0.207 - ( 0.08 * sin__q_RF_HAA__)) * sin__q_RF_HFE__) - (( 0.3735 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + (((((- 0.3735 * sin__q_RF_HAA__) * sin__q_RF_HFE__) + ((( 0.08 * sin__q_RF_HAA__) - 0.207) * cos__q_RF_HFE__)) + ( 0.35 * sin__q_RF_HAA__)) * cos__q_RF_KFE__)) + ( 0.33 * sin__q_RF_HAA__));
+ (*this)(5,2) = ((((((- 0.33 * cos__q_RF_HAA__) * cos__q_RF_HFE__) * sin__q_RF_KFE__) - ((( 0.33 * cos__q_RF_HAA__) * sin__q_RF_HFE__) * cos__q_RF_KFE__)) - (( 0.35 * cos__q_RF_HAA__) * sin__q_RF_HFE__)) + ( 0.3735 * cos__q_RF_HAA__));
+ (*this)(5,3) = ((( cos__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__) - (( cos__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(5,4) = ((( cos__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) + (( cos__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(5,5) = sin__q_RF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_foot::Type_fr_trunk_X_fr_LH_foot()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_foot& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_foot::update(const JState& q) {
+ Scalar sin__q_LH_KFE__;
+ Scalar sin__q_LH_HFE__;
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+ Scalar cos__q_LH_KFE__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_KFE__ = TRAIT::sin( q(LH_KFE));
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+ cos__q_LH_KFE__ = TRAIT::cos( q(LH_KFE));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,0) = ((- cos__q_LH_HFE__ * sin__q_LH_KFE__) - ( sin__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(0,1) = (( sin__q_LH_HFE__ * sin__q_LH_KFE__) - ( cos__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(1,0) = ((( sin__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( sin__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,1) = ((( sin__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( sin__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,2) = cos__q_LH_HAA__;
+ (*this)(2,0) = ((( cos__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( cos__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(2,1) = ((( cos__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( cos__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(3,0) = (((( 0.207 * cos__q_LH_HAA__) * sin__q_LH_HFE__) * sin__q_LH_KFE__) - ((( 0.207 * cos__q_LH_HAA__) * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(3,1) = (((( 0.207 * cos__q_LH_HAA__) * cos__q_LH_HFE__) * sin__q_LH_KFE__) + ((( 0.207 * cos__q_LH_HAA__) * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(3,2) = ((((((- 0.33 * sin__q_LH_HFE__) * sin__q_LH_KFE__) + (( 0.33 * cos__q_LH_HFE__) * cos__q_LH_KFE__)) + ( 0.35 * cos__q_LH_HFE__)) - ( 0.207 * sin__q_LH_HAA__)) + 0.08);
+ (*this)(3,3) = ((- cos__q_LH_HFE__ * sin__q_LH_KFE__) - ( sin__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(3,4) = (( sin__q_LH_HFE__ * sin__q_LH_KFE__) - ( cos__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(4,0) = (((((( 0.3735 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.08 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) + ( 0.35 * cos__q_LH_HAA__)) * sin__q_LH_KFE__) + (((( 0.08 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.3735 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) * cos__q_LH_KFE__));
+ (*this)(4,1) = (((((( 0.3735 * cos__q_LH_HAA__) * cos__q_LH_HFE__) - (( 0.08 * cos__q_LH_HAA__) * sin__q_LH_HFE__)) * sin__q_LH_KFE__) + ((((( 0.3735 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.08 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) + ( 0.35 * cos__q_LH_HAA__)) * cos__q_LH_KFE__)) + ( 0.33 * cos__q_LH_HAA__));
+ (*this)(4,2) = ((((((- 0.33 * sin__q_LH_HAA__) * cos__q_LH_HFE__) * sin__q_LH_KFE__) - ((( 0.33 * sin__q_LH_HAA__) * sin__q_LH_HFE__) * cos__q_LH_KFE__)) - (( 0.35 * sin__q_LH_HAA__) * sin__q_LH_HFE__)) - ( 0.3735 * sin__q_LH_HAA__));
+ (*this)(4,3) = ((( sin__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( sin__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(4,4) = ((( sin__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( sin__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(4,5) = cos__q_LH_HAA__;
+ (*this)(5,0) = ((((((- 0.3735 * sin__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * cos__q_LH_HFE__)) - ( 0.35 * sin__q_LH_HAA__)) * sin__q_LH_KFE__) + (((( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * sin__q_LH_HFE__) + (( 0.3735 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) * cos__q_LH_KFE__));
+ (*this)(5,1) = ((((((( 0.08 * sin__q_LH_HAA__) - 0.207) * sin__q_LH_HFE__) - (( 0.3735 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) * sin__q_LH_KFE__) + (((((- 0.3735 * sin__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * cos__q_LH_HFE__)) - ( 0.35 * sin__q_LH_HAA__)) * cos__q_LH_KFE__)) - ( 0.33 * sin__q_LH_HAA__));
+ (*this)(5,2) = ((((((- 0.33 * cos__q_LH_HAA__) * cos__q_LH_HFE__) * sin__q_LH_KFE__) - ((( 0.33 * cos__q_LH_HAA__) * sin__q_LH_HFE__) * cos__q_LH_KFE__)) - (( 0.35 * cos__q_LH_HAA__) * sin__q_LH_HFE__)) - ( 0.3735 * cos__q_LH_HAA__));
+ (*this)(5,3) = ((( cos__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( cos__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(5,4) = ((( cos__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( cos__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(5,5) = - sin__q_LH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_foot::Type_fr_trunk_X_fr_RH_foot()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_foot& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_foot::update(const JState& q) {
+ Scalar sin__q_RH_KFE__;
+ Scalar sin__q_RH_HFE__;
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+ Scalar cos__q_RH_KFE__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_KFE__ = TRAIT::sin( q(RH_KFE));
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+ cos__q_RH_KFE__ = TRAIT::cos( q(RH_KFE));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,0) = ((- cos__q_RH_HFE__ * sin__q_RH_KFE__) - ( sin__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(0,1) = (( sin__q_RH_HFE__ * sin__q_RH_KFE__) - ( cos__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(1,0) = ((( sin__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(1,1) = (((- sin__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(1,2) = cos__q_RH_HAA__;
+ (*this)(2,0) = ((( cos__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__) - (( cos__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(2,1) = ((( cos__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) + (( cos__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(3,0) = (((( 0.207 * cos__q_RH_HAA__) * cos__q_RH_HFE__) * cos__q_RH_KFE__) - ((( 0.207 * cos__q_RH_HAA__) * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(3,1) = ((((- 0.207 * cos__q_RH_HAA__) * cos__q_RH_HFE__) * sin__q_RH_KFE__) - ((( 0.207 * cos__q_RH_HAA__) * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(3,2) = ((((((- 0.33 * sin__q_RH_HFE__) * sin__q_RH_KFE__) + (( 0.33 * cos__q_RH_HFE__) * cos__q_RH_KFE__)) + ( 0.35 * cos__q_RH_HFE__)) - ( 0.207 * sin__q_RH_HAA__)) + 0.08);
+ (*this)(3,3) = ((- cos__q_RH_HFE__ * sin__q_RH_KFE__) - ( sin__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(3,4) = (( sin__q_RH_HFE__ * sin__q_RH_KFE__) - ( cos__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(4,0) = (((((( 0.3735 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.08 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.35 * cos__q_RH_HAA__)) * sin__q_RH_KFE__) + (((( 0.08 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.3735 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) * cos__q_RH_KFE__));
+ (*this)(4,1) = (((((( 0.3735 * cos__q_RH_HAA__) * cos__q_RH_HFE__) - (( 0.08 * cos__q_RH_HAA__) * sin__q_RH_HFE__)) * sin__q_RH_KFE__) + ((((( 0.3735 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.08 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.35 * cos__q_RH_HAA__)) * cos__q_RH_KFE__)) + ( 0.33 * cos__q_RH_HAA__));
+ (*this)(4,2) = (((((( 0.33 * sin__q_RH_HAA__) * cos__q_RH_HFE__) * sin__q_RH_KFE__) + ((( 0.33 * sin__q_RH_HAA__) * sin__q_RH_HFE__) * cos__q_RH_KFE__)) + (( 0.35 * sin__q_RH_HAA__) * sin__q_RH_HFE__)) + ( 0.3735 * sin__q_RH_HAA__));
+ (*this)(4,3) = ((( sin__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(4,4) = (((- sin__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(4,5) = cos__q_RH_HAA__;
+ (*this)(5,0) = (((((( 0.3735 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ((( 0.08 * sin__q_RH_HAA__) - 0.207) * cos__q_RH_HFE__)) + ( 0.35 * sin__q_RH_HAA__)) * sin__q_RH_KFE__) + ((((( 0.08 * sin__q_RH_HAA__) - 0.207) * sin__q_RH_HFE__) - (( 0.3735 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) * cos__q_RH_KFE__));
+ (*this)(5,1) = (((((( 0.207 - ( 0.08 * sin__q_RH_HAA__)) * sin__q_RH_HFE__) + (( 0.3735 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) * sin__q_RH_KFE__) + ((((( 0.3735 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ((( 0.08 * sin__q_RH_HAA__) - 0.207) * cos__q_RH_HFE__)) + ( 0.35 * sin__q_RH_HAA__)) * cos__q_RH_KFE__)) + ( 0.33 * sin__q_RH_HAA__));
+ (*this)(5,2) = ((((((- 0.33 * cos__q_RH_HAA__) * cos__q_RH_HFE__) * sin__q_RH_KFE__) - ((( 0.33 * cos__q_RH_HAA__) * sin__q_RH_HFE__) * cos__q_RH_KFE__)) - (( 0.35 * cos__q_RH_HAA__) * sin__q_RH_HFE__)) - ( 0.3735 * cos__q_RH_HAA__));
+ (*this)(5,3) = ((( cos__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__) - (( cos__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(5,4) = ((( cos__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) + (( cos__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(5,5) = sin__q_RH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LF_foot_X_fr_trunk::Type_fr_LF_foot_X_fr_trunk()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(5,3) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LF_foot_X_fr_trunk& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LF_foot_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_LF_KFE__;
+ Scalar sin__q_LF_HFE__;
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+ Scalar cos__q_LF_KFE__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_KFE__ = TRAIT::sin( q(LF_KFE));
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+ cos__q_LF_KFE__ = TRAIT::cos( q(LF_KFE));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,0) = ((- cos__q_LF_HFE__ * sin__q_LF_KFE__) - ( sin__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(0,1) = ((( sin__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( sin__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(0,2) = ((( cos__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( cos__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,0) = (( sin__q_LF_HFE__ * sin__q_LF_KFE__) - ( cos__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(1,1) = ((( sin__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( sin__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,2) = ((( cos__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( cos__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(2,1) = cos__q_LF_HAA__;
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(3,0) = (((( 0.207 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * sin__q_LF_KFE__) - ((( 0.207 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(3,1) = ((((((- 0.3735 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.08 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.35 * cos__q_LF_HAA__)) * sin__q_LF_KFE__) + (((( 0.08 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.3735 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) * cos__q_LF_KFE__));
+ (*this)(3,2) = (((((( 0.3735 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * cos__q_LF_HFE__)) - ( 0.35 * sin__q_LF_HAA__)) * sin__q_LF_KFE__) + (((( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * sin__q_LF_HFE__) - (( 0.3735 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) * cos__q_LF_KFE__));
+ (*this)(3,3) = ((- cos__q_LF_HFE__ * sin__q_LF_KFE__) - ( sin__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(3,4) = ((( sin__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( sin__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(3,5) = ((( cos__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( cos__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(4,0) = (((( 0.207 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) + ((( 0.207 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(4,1) = ((((((- 0.08 * cos__q_LF_HAA__) * sin__q_LF_HFE__) - (( 0.3735 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) * sin__q_LF_KFE__) + (((((- 0.3735 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.08 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.35 * cos__q_LF_HAA__)) * cos__q_LF_KFE__)) + ( 0.33 * cos__q_LF_HAA__));
+ (*this)(4,2) = ((((((( 0.08 * sin__q_LF_HAA__) - 0.207) * sin__q_LF_HFE__) + (( 0.3735 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) * sin__q_LF_KFE__) + ((((( 0.3735 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * cos__q_LF_HFE__)) - ( 0.35 * sin__q_LF_HAA__)) * cos__q_LF_KFE__)) - ( 0.33 * sin__q_LF_HAA__));
+ (*this)(4,3) = (( sin__q_LF_HFE__ * sin__q_LF_KFE__) - ( cos__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(4,4) = ((( sin__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( sin__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(4,5) = ((( cos__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( cos__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(5,0) = ((((((- 0.33 * sin__q_LF_HFE__) * sin__q_LF_KFE__) + (( 0.33 * cos__q_LF_HFE__) * cos__q_LF_KFE__)) + ( 0.35 * cos__q_LF_HFE__)) - ( 0.207 * sin__q_LF_HAA__)) + 0.08);
+ (*this)(5,1) = ((((((- 0.33 * sin__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) - ((( 0.33 * sin__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__)) - (( 0.35 * sin__q_LF_HAA__) * sin__q_LF_HFE__)) + ( 0.3735 * sin__q_LF_HAA__));
+ (*this)(5,2) = ((((((- 0.33 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) - ((( 0.33 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__)) - (( 0.35 * cos__q_LF_HAA__) * sin__q_LF_HFE__)) + ( 0.3735 * cos__q_LF_HAA__));
+ (*this)(5,4) = cos__q_LF_HAA__;
+ (*this)(5,5) = - sin__q_LF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RF_foot_X_fr_trunk::Type_fr_RF_foot_X_fr_trunk()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(5,3) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RF_foot_X_fr_trunk& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RF_foot_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_RF_KFE__;
+ Scalar sin__q_RF_HFE__;
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+ Scalar cos__q_RF_KFE__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_KFE__ = TRAIT::sin( q(RF_KFE));
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+ cos__q_RF_KFE__ = TRAIT::cos( q(RF_KFE));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,0) = ((- cos__q_RF_HFE__ * sin__q_RF_KFE__) - ( sin__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(0,1) = ((( sin__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(0,2) = ((( cos__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__) - (( cos__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(1,0) = (( sin__q_RF_HFE__ * sin__q_RF_KFE__) - ( cos__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(1,1) = (((- sin__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(1,2) = ((( cos__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) + (( cos__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(2,1) = cos__q_RF_HAA__;
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(3,0) = (((( 0.207 * cos__q_RF_HAA__) * cos__q_RF_HFE__) * cos__q_RF_KFE__) - ((( 0.207 * cos__q_RF_HAA__) * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(3,1) = ((((((- 0.3735 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.08 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.35 * cos__q_RF_HAA__)) * sin__q_RF_KFE__) + (((( 0.08 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.3735 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) * cos__q_RF_KFE__));
+ (*this)(3,2) = ((((((- 0.3735 * sin__q_RF_HAA__) * sin__q_RF_HFE__) + ((( 0.08 * sin__q_RF_HAA__) - 0.207) * cos__q_RF_HFE__)) + ( 0.35 * sin__q_RF_HAA__)) * sin__q_RF_KFE__) + ((((( 0.08 * sin__q_RF_HAA__) - 0.207) * sin__q_RF_HFE__) + (( 0.3735 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) * cos__q_RF_KFE__));
+ (*this)(3,3) = ((- cos__q_RF_HFE__ * sin__q_RF_KFE__) - ( sin__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(3,4) = ((( sin__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(3,5) = ((( cos__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__) - (( cos__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(4,0) = ((((- 0.207 * cos__q_RF_HAA__) * cos__q_RF_HFE__) * sin__q_RF_KFE__) - ((( 0.207 * cos__q_RF_HAA__) * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(4,1) = ((((((- 0.08 * cos__q_RF_HAA__) * sin__q_RF_HFE__) - (( 0.3735 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + (((((- 0.3735 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.08 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.35 * cos__q_RF_HAA__)) * cos__q_RF_KFE__)) + ( 0.33 * cos__q_RF_HAA__));
+ (*this)(4,2) = (((((( 0.207 - ( 0.08 * sin__q_RF_HAA__)) * sin__q_RF_HFE__) - (( 0.3735 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + (((((- 0.3735 * sin__q_RF_HAA__) * sin__q_RF_HFE__) + ((( 0.08 * sin__q_RF_HAA__) - 0.207) * cos__q_RF_HFE__)) + ( 0.35 * sin__q_RF_HAA__)) * cos__q_RF_KFE__)) + ( 0.33 * sin__q_RF_HAA__));
+ (*this)(4,3) = (( sin__q_RF_HFE__ * sin__q_RF_KFE__) - ( cos__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(4,4) = (((- sin__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(4,5) = ((( cos__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) + (( cos__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(5,0) = ((((((- 0.33 * sin__q_RF_HFE__) * sin__q_RF_KFE__) + (( 0.33 * cos__q_RF_HFE__) * cos__q_RF_KFE__)) + ( 0.35 * cos__q_RF_HFE__)) - ( 0.207 * sin__q_RF_HAA__)) + 0.08);
+ (*this)(5,1) = (((((( 0.33 * sin__q_RF_HAA__) * cos__q_RF_HFE__) * sin__q_RF_KFE__) + ((( 0.33 * sin__q_RF_HAA__) * sin__q_RF_HFE__) * cos__q_RF_KFE__)) + (( 0.35 * sin__q_RF_HAA__) * sin__q_RF_HFE__)) - ( 0.3735 * sin__q_RF_HAA__));
+ (*this)(5,2) = ((((((- 0.33 * cos__q_RF_HAA__) * cos__q_RF_HFE__) * sin__q_RF_KFE__) - ((( 0.33 * cos__q_RF_HAA__) * sin__q_RF_HFE__) * cos__q_RF_KFE__)) - (( 0.35 * cos__q_RF_HAA__) * sin__q_RF_HFE__)) + ( 0.3735 * cos__q_RF_HAA__));
+ (*this)(5,4) = cos__q_RF_HAA__;
+ (*this)(5,5) = sin__q_RF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LH_foot_X_fr_trunk::Type_fr_LH_foot_X_fr_trunk()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(5,3) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LH_foot_X_fr_trunk& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LH_foot_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_LH_KFE__;
+ Scalar sin__q_LH_HFE__;
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+ Scalar cos__q_LH_KFE__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_KFE__ = TRAIT::sin( q(LH_KFE));
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+ cos__q_LH_KFE__ = TRAIT::cos( q(LH_KFE));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,0) = ((- cos__q_LH_HFE__ * sin__q_LH_KFE__) - ( sin__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(0,1) = ((( sin__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( sin__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(0,2) = ((( cos__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( cos__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,0) = (( sin__q_LH_HFE__ * sin__q_LH_KFE__) - ( cos__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(1,1) = ((( sin__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( sin__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,2) = ((( cos__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( cos__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(2,1) = cos__q_LH_HAA__;
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(3,0) = (((( 0.207 * cos__q_LH_HAA__) * sin__q_LH_HFE__) * sin__q_LH_KFE__) - ((( 0.207 * cos__q_LH_HAA__) * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(3,1) = (((((( 0.3735 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.08 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) + ( 0.35 * cos__q_LH_HAA__)) * sin__q_LH_KFE__) + (((( 0.08 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.3735 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) * cos__q_LH_KFE__));
+ (*this)(3,2) = ((((((- 0.3735 * sin__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * cos__q_LH_HFE__)) - ( 0.35 * sin__q_LH_HAA__)) * sin__q_LH_KFE__) + (((( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * sin__q_LH_HFE__) + (( 0.3735 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) * cos__q_LH_KFE__));
+ (*this)(3,3) = ((- cos__q_LH_HFE__ * sin__q_LH_KFE__) - ( sin__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(3,4) = ((( sin__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( sin__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(3,5) = ((( cos__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( cos__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(4,0) = (((( 0.207 * cos__q_LH_HAA__) * cos__q_LH_HFE__) * sin__q_LH_KFE__) + ((( 0.207 * cos__q_LH_HAA__) * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(4,1) = (((((( 0.3735 * cos__q_LH_HAA__) * cos__q_LH_HFE__) - (( 0.08 * cos__q_LH_HAA__) * sin__q_LH_HFE__)) * sin__q_LH_KFE__) + ((((( 0.3735 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.08 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) + ( 0.35 * cos__q_LH_HAA__)) * cos__q_LH_KFE__)) + ( 0.33 * cos__q_LH_HAA__));
+ (*this)(4,2) = ((((((( 0.08 * sin__q_LH_HAA__) - 0.207) * sin__q_LH_HFE__) - (( 0.3735 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) * sin__q_LH_KFE__) + (((((- 0.3735 * sin__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * cos__q_LH_HFE__)) - ( 0.35 * sin__q_LH_HAA__)) * cos__q_LH_KFE__)) - ( 0.33 * sin__q_LH_HAA__));
+ (*this)(4,3) = (( sin__q_LH_HFE__ * sin__q_LH_KFE__) - ( cos__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(4,4) = ((( sin__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( sin__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(4,5) = ((( cos__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( cos__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(5,0) = ((((((- 0.33 * sin__q_LH_HFE__) * sin__q_LH_KFE__) + (( 0.33 * cos__q_LH_HFE__) * cos__q_LH_KFE__)) + ( 0.35 * cos__q_LH_HFE__)) - ( 0.207 * sin__q_LH_HAA__)) + 0.08);
+ (*this)(5,1) = ((((((- 0.33 * sin__q_LH_HAA__) * cos__q_LH_HFE__) * sin__q_LH_KFE__) - ((( 0.33 * sin__q_LH_HAA__) * sin__q_LH_HFE__) * cos__q_LH_KFE__)) - (( 0.35 * sin__q_LH_HAA__) * sin__q_LH_HFE__)) - ( 0.3735 * sin__q_LH_HAA__));
+ (*this)(5,2) = ((((((- 0.33 * cos__q_LH_HAA__) * cos__q_LH_HFE__) * sin__q_LH_KFE__) - ((( 0.33 * cos__q_LH_HAA__) * sin__q_LH_HFE__) * cos__q_LH_KFE__)) - (( 0.35 * cos__q_LH_HAA__) * sin__q_LH_HFE__)) - ( 0.3735 * cos__q_LH_HAA__));
+ (*this)(5,4) = cos__q_LH_HAA__;
+ (*this)(5,5) = - sin__q_LH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RH_foot_X_fr_trunk::Type_fr_RH_foot_X_fr_trunk()
+{
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(5,3) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RH_foot_X_fr_trunk& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RH_foot_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_RH_KFE__;
+ Scalar sin__q_RH_HFE__;
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+ Scalar cos__q_RH_KFE__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_KFE__ = TRAIT::sin( q(RH_KFE));
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+ cos__q_RH_KFE__ = TRAIT::cos( q(RH_KFE));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,0) = ((- cos__q_RH_HFE__ * sin__q_RH_KFE__) - ( sin__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(0,1) = ((( sin__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(0,2) = ((( cos__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__) - (( cos__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(1,0) = (( sin__q_RH_HFE__ * sin__q_RH_KFE__) - ( cos__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(1,1) = (((- sin__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(1,2) = ((( cos__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) + (( cos__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(2,1) = cos__q_RH_HAA__;
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(3,0) = (((( 0.207 * cos__q_RH_HAA__) * cos__q_RH_HFE__) * cos__q_RH_KFE__) - ((( 0.207 * cos__q_RH_HAA__) * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(3,1) = (((((( 0.3735 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.08 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.35 * cos__q_RH_HAA__)) * sin__q_RH_KFE__) + (((( 0.08 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.3735 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) * cos__q_RH_KFE__));
+ (*this)(3,2) = (((((( 0.3735 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ((( 0.08 * sin__q_RH_HAA__) - 0.207) * cos__q_RH_HFE__)) + ( 0.35 * sin__q_RH_HAA__)) * sin__q_RH_KFE__) + ((((( 0.08 * sin__q_RH_HAA__) - 0.207) * sin__q_RH_HFE__) - (( 0.3735 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) * cos__q_RH_KFE__));
+ (*this)(3,3) = ((- cos__q_RH_HFE__ * sin__q_RH_KFE__) - ( sin__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(3,4) = ((( sin__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(3,5) = ((( cos__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__) - (( cos__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(4,0) = ((((- 0.207 * cos__q_RH_HAA__) * cos__q_RH_HFE__) * sin__q_RH_KFE__) - ((( 0.207 * cos__q_RH_HAA__) * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(4,1) = (((((( 0.3735 * cos__q_RH_HAA__) * cos__q_RH_HFE__) - (( 0.08 * cos__q_RH_HAA__) * sin__q_RH_HFE__)) * sin__q_RH_KFE__) + ((((( 0.3735 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.08 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.35 * cos__q_RH_HAA__)) * cos__q_RH_KFE__)) + ( 0.33 * cos__q_RH_HAA__));
+ (*this)(4,2) = (((((( 0.207 - ( 0.08 * sin__q_RH_HAA__)) * sin__q_RH_HFE__) + (( 0.3735 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) * sin__q_RH_KFE__) + ((((( 0.3735 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ((( 0.08 * sin__q_RH_HAA__) - 0.207) * cos__q_RH_HFE__)) + ( 0.35 * sin__q_RH_HAA__)) * cos__q_RH_KFE__)) + ( 0.33 * sin__q_RH_HAA__));
+ (*this)(4,3) = (( sin__q_RH_HFE__ * sin__q_RH_KFE__) - ( cos__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(4,4) = (((- sin__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(4,5) = ((( cos__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) + (( cos__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(5,0) = ((((((- 0.33 * sin__q_RH_HFE__) * sin__q_RH_KFE__) + (( 0.33 * cos__q_RH_HFE__) * cos__q_RH_KFE__)) + ( 0.35 * cos__q_RH_HFE__)) - ( 0.207 * sin__q_RH_HAA__)) + 0.08);
+ (*this)(5,1) = (((((( 0.33 * sin__q_RH_HAA__) * cos__q_RH_HFE__) * sin__q_RH_KFE__) + ((( 0.33 * sin__q_RH_HAA__) * sin__q_RH_HFE__) * cos__q_RH_KFE__)) + (( 0.35 * sin__q_RH_HAA__) * sin__q_RH_HFE__)) + ( 0.3735 * sin__q_RH_HAA__));
+ (*this)(5,2) = ((((((- 0.33 * cos__q_RH_HAA__) * cos__q_RH_HFE__) * sin__q_RH_KFE__) - ((( 0.33 * cos__q_RH_HAA__) * sin__q_RH_HFE__) * cos__q_RH_KFE__)) - (( 0.35 * cos__q_RH_HAA__) * sin__q_RH_HFE__)) - ( 0.3735 * cos__q_RH_HAA__));
+ (*this)(5,4) = cos__q_RH_HAA__;
+ (*this)(5,5) = sin__q_RH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_HAA::Type_fr_trunk_X_fr_LF_HAA()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = - 1.0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = - 1.0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = - 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = - 0.207;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = - 1.0;
+ (*this)(4,0) = 0.3735;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = - 1.0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = - 0.3735;
+ (*this)(5,2) = 0.207;
+ (*this)(5,3) = - 1.0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_HAA& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_HAA::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_HAA::Type_fr_trunk_X_fr_RF_HAA()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 1.0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 1.0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = - 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0.207;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 1.0;
+ (*this)(4,0) = 0.3735;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = 1.0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0.3735;
+ (*this)(5,2) = 0.207;
+ (*this)(5,3) = - 1.0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_HAA& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_HAA::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_HAA::Type_fr_trunk_X_fr_LH_HAA()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = - 1.0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = - 1.0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = - 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = - 0.207;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = - 1.0;
+ (*this)(4,0) = - 0.3735;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = - 1.0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0.3735;
+ (*this)(5,2) = 0.207;
+ (*this)(5,3) = - 1.0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_HAA& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_HAA::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_HAA::Type_fr_trunk_X_fr_RH_HAA()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 1.0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 1.0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = - 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0.207;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 1.0;
+ (*this)(4,0) = - 0.3735;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = 1.0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = - 0.3735;
+ (*this)(5,2) = 0.207;
+ (*this)(5,3) = - 1.0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_HAA& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_HAA::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_HFE::Type_fr_trunk_X_fr_LF_HFE()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = - 1.0;
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,1) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = - 1.0;
+ (*this)(3,5) = 0;
+ (*this)(4,4) = 0;
+ (*this)(5,4) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_HFE& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_HFE::update(const JState& q) {
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(1,0) = - sin__q_LF_HAA__;
+ (*this)(1,2) = cos__q_LF_HAA__;
+ (*this)(2,0) = - cos__q_LF_HAA__;
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(3,0) = (- 0.207 * cos__q_LF_HAA__);
+ (*this)(3,2) = ( 0.08 - ( 0.207 * sin__q_LF_HAA__));
+ (*this)(4,0) = ( 0.3735 * cos__q_LF_HAA__);
+ (*this)(4,1) = ( 0.08 * cos__q_LF_HAA__);
+ (*this)(4,2) = ( 0.3735 * sin__q_LF_HAA__);
+ (*this)(4,3) = - sin__q_LF_HAA__;
+ (*this)(4,5) = cos__q_LF_HAA__;
+ (*this)(5,0) = (- 0.3735 * sin__q_LF_HAA__);
+ (*this)(5,1) = ( 0.207 - ( 0.08 * sin__q_LF_HAA__));
+ (*this)(5,2) = ( 0.3735 * cos__q_LF_HAA__);
+ (*this)(5,3) = - cos__q_LF_HAA__;
+ (*this)(5,5) = - sin__q_LF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_HFE::Type_fr_trunk_X_fr_RF_HFE()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = - 1.0;
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,1) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = - 1.0;
+ (*this)(3,5) = 0;
+ (*this)(4,4) = 0;
+ (*this)(5,4) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_HFE& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_HFE::update(const JState& q) {
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(1,0) = sin__q_RF_HAA__;
+ (*this)(1,2) = cos__q_RF_HAA__;
+ (*this)(2,0) = - cos__q_RF_HAA__;
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(3,0) = ( 0.207 * cos__q_RF_HAA__);
+ (*this)(3,2) = ( 0.08 - ( 0.207 * sin__q_RF_HAA__));
+ (*this)(4,0) = ( 0.3735 * cos__q_RF_HAA__);
+ (*this)(4,1) = ( 0.08 * cos__q_RF_HAA__);
+ (*this)(4,2) = (- 0.3735 * sin__q_RF_HAA__);
+ (*this)(4,3) = sin__q_RF_HAA__;
+ (*this)(4,5) = cos__q_RF_HAA__;
+ (*this)(5,0) = ( 0.3735 * sin__q_RF_HAA__);
+ (*this)(5,1) = (( 0.08 * sin__q_RF_HAA__) - 0.207);
+ (*this)(5,2) = ( 0.3735 * cos__q_RF_HAA__);
+ (*this)(5,3) = - cos__q_RF_HAA__;
+ (*this)(5,5) = sin__q_RF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_HFE::Type_fr_trunk_X_fr_LH_HFE()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = - 1.0;
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,1) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = - 1.0;
+ (*this)(3,5) = 0;
+ (*this)(4,4) = 0;
+ (*this)(5,4) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_HFE& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_HFE::update(const JState& q) {
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(1,0) = - sin__q_LH_HAA__;
+ (*this)(1,2) = cos__q_LH_HAA__;
+ (*this)(2,0) = - cos__q_LH_HAA__;
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(3,0) = (- 0.207 * cos__q_LH_HAA__);
+ (*this)(3,2) = ( 0.08 - ( 0.207 * sin__q_LH_HAA__));
+ (*this)(4,0) = (- 0.3735 * cos__q_LH_HAA__);
+ (*this)(4,1) = ( 0.08 * cos__q_LH_HAA__);
+ (*this)(4,2) = (- 0.3735 * sin__q_LH_HAA__);
+ (*this)(4,3) = - sin__q_LH_HAA__;
+ (*this)(4,5) = cos__q_LH_HAA__;
+ (*this)(5,0) = ( 0.3735 * sin__q_LH_HAA__);
+ (*this)(5,1) = ( 0.207 - ( 0.08 * sin__q_LH_HAA__));
+ (*this)(5,2) = (- 0.3735 * cos__q_LH_HAA__);
+ (*this)(5,3) = - cos__q_LH_HAA__;
+ (*this)(5,5) = - sin__q_LH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_HFE::Type_fr_trunk_X_fr_RH_HFE()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = - 1.0;
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,1) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = - 1.0;
+ (*this)(3,5) = 0;
+ (*this)(4,4) = 0;
+ (*this)(5,4) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_HFE& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_HFE::update(const JState& q) {
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(1,0) = sin__q_RH_HAA__;
+ (*this)(1,2) = cos__q_RH_HAA__;
+ (*this)(2,0) = - cos__q_RH_HAA__;
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(3,0) = ( 0.207 * cos__q_RH_HAA__);
+ (*this)(3,2) = ( 0.08 - ( 0.207 * sin__q_RH_HAA__));
+ (*this)(4,0) = (- 0.3735 * cos__q_RH_HAA__);
+ (*this)(4,1) = ( 0.08 * cos__q_RH_HAA__);
+ (*this)(4,2) = ( 0.3735 * sin__q_RH_HAA__);
+ (*this)(4,3) = sin__q_RH_HAA__;
+ (*this)(4,5) = cos__q_RH_HAA__;
+ (*this)(5,0) = (- 0.3735 * sin__q_RH_HAA__);
+ (*this)(5,1) = (( 0.08 * sin__q_RH_HAA__) - 0.207);
+ (*this)(5,2) = (- 0.3735 * cos__q_RH_HAA__);
+ (*this)(5,3) = - cos__q_RH_HAA__;
+ (*this)(5,5) = sin__q_RH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_KFE::Type_fr_trunk_X_fr_LF_KFE()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_KFE& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_KFE::update(const JState& q) {
+ Scalar sin__q_LF_HFE__;
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,0) = - sin__q_LF_HFE__;
+ (*this)(0,1) = - cos__q_LF_HFE__;
+ (*this)(1,0) = (- sin__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(1,1) = ( sin__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(1,2) = cos__q_LF_HAA__;
+ (*this)(2,0) = (- cos__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(2,1) = ( cos__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(3,0) = ((- 0.207 * cos__q_LF_HAA__) * cos__q_LF_HFE__);
+ (*this)(3,1) = (( 0.207 * cos__q_LF_HAA__) * sin__q_LF_HFE__);
+ (*this)(3,2) = ((( 0.35 * cos__q_LF_HFE__) - ( 0.207 * sin__q_LF_HAA__)) + 0.08);
+ (*this)(3,3) = - sin__q_LF_HFE__;
+ (*this)(3,4) = - cos__q_LF_HFE__;
+ (*this)(4,0) = ((( 0.08 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.3735 * cos__q_LF_HAA__) * cos__q_LF_HFE__));
+ (*this)(4,1) = ((((- 0.3735 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.08 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.35 * cos__q_LF_HAA__));
+ (*this)(4,2) = (( 0.3735 * sin__q_LF_HAA__) - (( 0.35 * sin__q_LF_HAA__) * sin__q_LF_HFE__));
+ (*this)(4,3) = (- sin__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(4,4) = ( sin__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(4,5) = cos__q_LF_HAA__;
+ (*this)(5,0) = ((( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * sin__q_LF_HFE__) - (( 0.3735 * sin__q_LF_HAA__) * cos__q_LF_HFE__));
+ (*this)(5,1) = (((( 0.3735 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * cos__q_LF_HFE__)) - ( 0.35 * sin__q_LF_HAA__));
+ (*this)(5,2) = (( 0.3735 * cos__q_LF_HAA__) - (( 0.35 * cos__q_LF_HAA__) * sin__q_LF_HFE__));
+ (*this)(5,3) = (- cos__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(5,4) = ( cos__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(5,5) = - sin__q_LF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_KFE::Type_fr_trunk_X_fr_RF_KFE()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_KFE& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_KFE::update(const JState& q) {
+ Scalar sin__q_RF_HFE__;
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,0) = - sin__q_RF_HFE__;
+ (*this)(0,1) = - cos__q_RF_HFE__;
+ (*this)(1,0) = ( sin__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(1,1) = (- sin__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(1,2) = cos__q_RF_HAA__;
+ (*this)(2,0) = (- cos__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(2,1) = ( cos__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(3,0) = (( 0.207 * cos__q_RF_HAA__) * cos__q_RF_HFE__);
+ (*this)(3,1) = ((- 0.207 * cos__q_RF_HAA__) * sin__q_RF_HFE__);
+ (*this)(3,2) = ((( 0.35 * cos__q_RF_HFE__) - ( 0.207 * sin__q_RF_HAA__)) + 0.08);
+ (*this)(3,3) = - sin__q_RF_HFE__;
+ (*this)(3,4) = - cos__q_RF_HFE__;
+ (*this)(4,0) = ((( 0.08 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.3735 * cos__q_RF_HAA__) * cos__q_RF_HFE__));
+ (*this)(4,1) = ((((- 0.3735 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.08 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.35 * cos__q_RF_HAA__));
+ (*this)(4,2) = ((( 0.35 * sin__q_RF_HAA__) * sin__q_RF_HFE__) - ( 0.3735 * sin__q_RF_HAA__));
+ (*this)(4,3) = ( sin__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(4,4) = (- sin__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(4,5) = cos__q_RF_HAA__;
+ (*this)(5,0) = (((( 0.08 * sin__q_RF_HAA__) - 0.207) * sin__q_RF_HFE__) + (( 0.3735 * sin__q_RF_HAA__) * cos__q_RF_HFE__));
+ (*this)(5,1) = ((((- 0.3735 * sin__q_RF_HAA__) * sin__q_RF_HFE__) + ((( 0.08 * sin__q_RF_HAA__) - 0.207) * cos__q_RF_HFE__)) + ( 0.35 * sin__q_RF_HAA__));
+ (*this)(5,2) = (( 0.3735 * cos__q_RF_HAA__) - (( 0.35 * cos__q_RF_HAA__) * sin__q_RF_HFE__));
+ (*this)(5,3) = (- cos__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(5,4) = ( cos__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(5,5) = sin__q_RF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_KFE::Type_fr_trunk_X_fr_LH_KFE()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_KFE& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_KFE::update(const JState& q) {
+ Scalar sin__q_LH_HFE__;
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,0) = - sin__q_LH_HFE__;
+ (*this)(0,1) = - cos__q_LH_HFE__;
+ (*this)(1,0) = (- sin__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(1,1) = ( sin__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(1,2) = cos__q_LH_HAA__;
+ (*this)(2,0) = (- cos__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(2,1) = ( cos__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(3,0) = ((- 0.207 * cos__q_LH_HAA__) * cos__q_LH_HFE__);
+ (*this)(3,1) = (( 0.207 * cos__q_LH_HAA__) * sin__q_LH_HFE__);
+ (*this)(3,2) = ((( 0.35 * cos__q_LH_HFE__) - ( 0.207 * sin__q_LH_HAA__)) + 0.08);
+ (*this)(3,3) = - sin__q_LH_HFE__;
+ (*this)(3,4) = - cos__q_LH_HFE__;
+ (*this)(4,0) = ((( 0.08 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.3735 * cos__q_LH_HAA__) * cos__q_LH_HFE__));
+ (*this)(4,1) = (((( 0.3735 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.08 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) + ( 0.35 * cos__q_LH_HAA__));
+ (*this)(4,2) = (((- 0.35 * sin__q_LH_HAA__) * sin__q_LH_HFE__) - ( 0.3735 * sin__q_LH_HAA__));
+ (*this)(4,3) = (- sin__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(4,4) = ( sin__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(4,5) = cos__q_LH_HAA__;
+ (*this)(5,0) = ((( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * sin__q_LH_HFE__) + (( 0.3735 * sin__q_LH_HAA__) * cos__q_LH_HFE__));
+ (*this)(5,1) = ((((- 0.3735 * sin__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * cos__q_LH_HFE__)) - ( 0.35 * sin__q_LH_HAA__));
+ (*this)(5,2) = (((- 0.35 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - ( 0.3735 * cos__q_LH_HAA__));
+ (*this)(5,3) = (- cos__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(5,4) = ( cos__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(5,5) = - sin__q_LH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_KFE::Type_fr_trunk_X_fr_RH_KFE()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_KFE& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_KFE::update(const JState& q) {
+ Scalar sin__q_RH_HFE__;
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,0) = - sin__q_RH_HFE__;
+ (*this)(0,1) = - cos__q_RH_HFE__;
+ (*this)(1,0) = ( sin__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(1,1) = (- sin__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(1,2) = cos__q_RH_HAA__;
+ (*this)(2,0) = (- cos__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(2,1) = ( cos__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(3,0) = (( 0.207 * cos__q_RH_HAA__) * cos__q_RH_HFE__);
+ (*this)(3,1) = ((- 0.207 * cos__q_RH_HAA__) * sin__q_RH_HFE__);
+ (*this)(3,2) = ((( 0.35 * cos__q_RH_HFE__) - ( 0.207 * sin__q_RH_HAA__)) + 0.08);
+ (*this)(3,3) = - sin__q_RH_HFE__;
+ (*this)(3,4) = - cos__q_RH_HFE__;
+ (*this)(4,0) = ((( 0.08 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.3735 * cos__q_RH_HAA__) * cos__q_RH_HFE__));
+ (*this)(4,1) = (((( 0.3735 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.08 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.35 * cos__q_RH_HAA__));
+ (*this)(4,2) = ((( 0.35 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ( 0.3735 * sin__q_RH_HAA__));
+ (*this)(4,3) = ( sin__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(4,4) = (- sin__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(4,5) = cos__q_RH_HAA__;
+ (*this)(5,0) = (((( 0.08 * sin__q_RH_HAA__) - 0.207) * sin__q_RH_HFE__) - (( 0.3735 * sin__q_RH_HAA__) * cos__q_RH_HFE__));
+ (*this)(5,1) = (((( 0.3735 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ((( 0.08 * sin__q_RH_HAA__) - 0.207) * cos__q_RH_HFE__)) + ( 0.35 * sin__q_RH_HAA__));
+ (*this)(5,2) = (((- 0.35 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - ( 0.3735 * cos__q_RH_HAA__));
+ (*this)(5,3) = (- cos__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(5,4) = ( cos__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(5,5) = sin__q_RH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LF_upperleg_X_fr_LF_hipassembly::Type_fr_LF_upperleg_X_fr_LF_hipassembly()
+{
+ (*this)(0,1) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,1) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = - 1.0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,4) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,4) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = - 0.08;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = - 1;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LF_upperleg_X_fr_LF_hipassembly& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LF_upperleg_X_fr_LF_hipassembly::update(const JState& q) {
+ Scalar sin__q_LF_HFE__;
+ Scalar cos__q_LF_HFE__;
+
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+
+ (*this)(0,0) = cos__q_LF_HFE__;
+ (*this)(0,2) = sin__q_LF_HFE__;
+ (*this)(1,0) = - sin__q_LF_HFE__;
+ (*this)(1,2) = cos__q_LF_HFE__;
+ (*this)(3,1) = (- 0.08 * sin__q_LF_HFE__);
+ (*this)(3,3) = cos__q_LF_HFE__;
+ (*this)(3,5) = sin__q_LF_HFE__;
+ (*this)(4,1) = (- 0.08 * cos__q_LF_HFE__);
+ (*this)(4,3) = - sin__q_LF_HFE__;
+ (*this)(4,5) = cos__q_LF_HFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LF_hipassembly_X_fr_LF_upperleg::Type_fr_LF_hipassembly_X_fr_LF_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 0;
+ (*this)(1,2) = - 1;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = 0;
+ (*this)(4,5) = - 1.0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = - 0.08;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LF_hipassembly_X_fr_LF_upperleg& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LF_hipassembly_X_fr_LF_upperleg::update(const JState& q) {
+ Scalar sin__q_LF_HFE__;
+ Scalar cos__q_LF_HFE__;
+
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+
+ (*this)(0,0) = cos__q_LF_HFE__;
+ (*this)(0,1) = - sin__q_LF_HFE__;
+ (*this)(2,0) = sin__q_LF_HFE__;
+ (*this)(2,1) = cos__q_LF_HFE__;
+ (*this)(3,3) = cos__q_LF_HFE__;
+ (*this)(3,4) = - sin__q_LF_HFE__;
+ (*this)(4,0) = (- 0.08 * sin__q_LF_HFE__);
+ (*this)(4,1) = (- 0.08 * cos__q_LF_HFE__);
+ (*this)(5,3) = sin__q_LF_HFE__;
+ (*this)(5,4) = cos__q_LF_HFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LF_lowerleg_X_fr_LF_upperleg::Type_fr_LF_lowerleg_X_fr_LF_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1.0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = - 0.35;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LF_lowerleg_X_fr_LF_upperleg& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LF_lowerleg_X_fr_LF_upperleg::update(const JState& q) {
+ Scalar sin__q_LF_KFE__;
+ Scalar cos__q_LF_KFE__;
+
+ sin__q_LF_KFE__ = TRAIT::sin( q(LF_KFE));
+ cos__q_LF_KFE__ = TRAIT::cos( q(LF_KFE));
+
+ (*this)(0,0) = cos__q_LF_KFE__;
+ (*this)(0,1) = sin__q_LF_KFE__;
+ (*this)(1,0) = - sin__q_LF_KFE__;
+ (*this)(1,1) = cos__q_LF_KFE__;
+ (*this)(3,2) = ( 0.35 * sin__q_LF_KFE__);
+ (*this)(3,3) = cos__q_LF_KFE__;
+ (*this)(3,4) = sin__q_LF_KFE__;
+ (*this)(4,2) = ( 0.35 * cos__q_LF_KFE__);
+ (*this)(4,3) = - sin__q_LF_KFE__;
+ (*this)(4,4) = cos__q_LF_KFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LF_upperleg_X_fr_LF_lowerleg::Type_fr_LF_upperleg_X_fr_LF_lowerleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = - 0.35;
+ (*this)(4,5) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LF_upperleg_X_fr_LF_lowerleg& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LF_upperleg_X_fr_LF_lowerleg::update(const JState& q) {
+ Scalar sin__q_LF_KFE__;
+ Scalar cos__q_LF_KFE__;
+
+ sin__q_LF_KFE__ = TRAIT::sin( q(LF_KFE));
+ cos__q_LF_KFE__ = TRAIT::cos( q(LF_KFE));
+
+ (*this)(0,0) = cos__q_LF_KFE__;
+ (*this)(0,1) = - sin__q_LF_KFE__;
+ (*this)(1,0) = sin__q_LF_KFE__;
+ (*this)(1,1) = cos__q_LF_KFE__;
+ (*this)(3,3) = cos__q_LF_KFE__;
+ (*this)(3,4) = - sin__q_LF_KFE__;
+ (*this)(4,3) = sin__q_LF_KFE__;
+ (*this)(4,4) = cos__q_LF_KFE__;
+ (*this)(5,0) = ( 0.35 * sin__q_LF_KFE__);
+ (*this)(5,1) = ( 0.35 * cos__q_LF_KFE__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RF_upperleg_X_fr_RF_hipassembly::Type_fr_RF_upperleg_X_fr_RF_hipassembly()
+{
+ (*this)(0,1) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,1) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 1.0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,4) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,4) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0.08;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 1;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RF_upperleg_X_fr_RF_hipassembly& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RF_upperleg_X_fr_RF_hipassembly::update(const JState& q) {
+ Scalar sin__q_RF_HFE__;
+ Scalar cos__q_RF_HFE__;
+
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+
+ (*this)(0,0) = cos__q_RF_HFE__;
+ (*this)(0,2) = - sin__q_RF_HFE__;
+ (*this)(1,0) = - sin__q_RF_HFE__;
+ (*this)(1,2) = - cos__q_RF_HFE__;
+ (*this)(3,1) = ( 0.08 * sin__q_RF_HFE__);
+ (*this)(3,3) = cos__q_RF_HFE__;
+ (*this)(3,5) = - sin__q_RF_HFE__;
+ (*this)(4,1) = ( 0.08 * cos__q_RF_HFE__);
+ (*this)(4,3) = - sin__q_RF_HFE__;
+ (*this)(4,5) = - cos__q_RF_HFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RF_hipassembly_X_fr_RF_upperleg::Type_fr_RF_hipassembly_X_fr_RF_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 0;
+ (*this)(1,2) = 1;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = 0;
+ (*this)(4,5) = 1.0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0.08;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RF_hipassembly_X_fr_RF_upperleg& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RF_hipassembly_X_fr_RF_upperleg::update(const JState& q) {
+ Scalar sin__q_RF_HFE__;
+ Scalar cos__q_RF_HFE__;
+
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+
+ (*this)(0,0) = cos__q_RF_HFE__;
+ (*this)(0,1) = - sin__q_RF_HFE__;
+ (*this)(2,0) = - sin__q_RF_HFE__;
+ (*this)(2,1) = - cos__q_RF_HFE__;
+ (*this)(3,3) = cos__q_RF_HFE__;
+ (*this)(3,4) = - sin__q_RF_HFE__;
+ (*this)(4,0) = ( 0.08 * sin__q_RF_HFE__);
+ (*this)(4,1) = ( 0.08 * cos__q_RF_HFE__);
+ (*this)(5,3) = - sin__q_RF_HFE__;
+ (*this)(5,4) = - cos__q_RF_HFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RF_lowerleg_X_fr_RF_upperleg::Type_fr_RF_lowerleg_X_fr_RF_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1.0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = - 0.35;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RF_lowerleg_X_fr_RF_upperleg& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RF_lowerleg_X_fr_RF_upperleg::update(const JState& q) {
+ Scalar sin__q_RF_KFE__;
+ Scalar cos__q_RF_KFE__;
+
+ sin__q_RF_KFE__ = TRAIT::sin( q(RF_KFE));
+ cos__q_RF_KFE__ = TRAIT::cos( q(RF_KFE));
+
+ (*this)(0,0) = cos__q_RF_KFE__;
+ (*this)(0,1) = sin__q_RF_KFE__;
+ (*this)(1,0) = - sin__q_RF_KFE__;
+ (*this)(1,1) = cos__q_RF_KFE__;
+ (*this)(3,2) = ( 0.35 * sin__q_RF_KFE__);
+ (*this)(3,3) = cos__q_RF_KFE__;
+ (*this)(3,4) = sin__q_RF_KFE__;
+ (*this)(4,2) = ( 0.35 * cos__q_RF_KFE__);
+ (*this)(4,3) = - sin__q_RF_KFE__;
+ (*this)(4,4) = cos__q_RF_KFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RF_upperleg_X_fr_RF_lowerleg::Type_fr_RF_upperleg_X_fr_RF_lowerleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = - 0.35;
+ (*this)(4,5) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RF_upperleg_X_fr_RF_lowerleg& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RF_upperleg_X_fr_RF_lowerleg::update(const JState& q) {
+ Scalar sin__q_RF_KFE__;
+ Scalar cos__q_RF_KFE__;
+
+ sin__q_RF_KFE__ = TRAIT::sin( q(RF_KFE));
+ cos__q_RF_KFE__ = TRAIT::cos( q(RF_KFE));
+
+ (*this)(0,0) = cos__q_RF_KFE__;
+ (*this)(0,1) = - sin__q_RF_KFE__;
+ (*this)(1,0) = sin__q_RF_KFE__;
+ (*this)(1,1) = cos__q_RF_KFE__;
+ (*this)(3,3) = cos__q_RF_KFE__;
+ (*this)(3,4) = - sin__q_RF_KFE__;
+ (*this)(4,3) = sin__q_RF_KFE__;
+ (*this)(4,4) = cos__q_RF_KFE__;
+ (*this)(5,0) = ( 0.35 * sin__q_RF_KFE__);
+ (*this)(5,1) = ( 0.35 * cos__q_RF_KFE__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LH_upperleg_X_fr_LH_hipassembly::Type_fr_LH_upperleg_X_fr_LH_hipassembly()
+{
+ (*this)(0,1) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,1) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = - 1.0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,4) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,4) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = - 0.08;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = - 1;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LH_upperleg_X_fr_LH_hipassembly& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LH_upperleg_X_fr_LH_hipassembly::update(const JState& q) {
+ Scalar sin__q_LH_HFE__;
+ Scalar cos__q_LH_HFE__;
+
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+
+ (*this)(0,0) = cos__q_LH_HFE__;
+ (*this)(0,2) = sin__q_LH_HFE__;
+ (*this)(1,0) = - sin__q_LH_HFE__;
+ (*this)(1,2) = cos__q_LH_HFE__;
+ (*this)(3,1) = (- 0.08 * sin__q_LH_HFE__);
+ (*this)(3,3) = cos__q_LH_HFE__;
+ (*this)(3,5) = sin__q_LH_HFE__;
+ (*this)(4,1) = (- 0.08 * cos__q_LH_HFE__);
+ (*this)(4,3) = - sin__q_LH_HFE__;
+ (*this)(4,5) = cos__q_LH_HFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LH_hipassembly_X_fr_LH_upperleg::Type_fr_LH_hipassembly_X_fr_LH_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 0;
+ (*this)(1,2) = - 1;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = 0;
+ (*this)(4,5) = - 1.0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = - 0.08;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LH_hipassembly_X_fr_LH_upperleg& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LH_hipassembly_X_fr_LH_upperleg::update(const JState& q) {
+ Scalar sin__q_LH_HFE__;
+ Scalar cos__q_LH_HFE__;
+
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+
+ (*this)(0,0) = cos__q_LH_HFE__;
+ (*this)(0,1) = - sin__q_LH_HFE__;
+ (*this)(2,0) = sin__q_LH_HFE__;
+ (*this)(2,1) = cos__q_LH_HFE__;
+ (*this)(3,3) = cos__q_LH_HFE__;
+ (*this)(3,4) = - sin__q_LH_HFE__;
+ (*this)(4,0) = (- 0.08 * sin__q_LH_HFE__);
+ (*this)(4,1) = (- 0.08 * cos__q_LH_HFE__);
+ (*this)(5,3) = sin__q_LH_HFE__;
+ (*this)(5,4) = cos__q_LH_HFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LH_lowerleg_X_fr_LH_upperleg::Type_fr_LH_lowerleg_X_fr_LH_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1.0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = - 0.35;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LH_lowerleg_X_fr_LH_upperleg& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LH_lowerleg_X_fr_LH_upperleg::update(const JState& q) {
+ Scalar sin__q_LH_KFE__;
+ Scalar cos__q_LH_KFE__;
+
+ sin__q_LH_KFE__ = TRAIT::sin( q(LH_KFE));
+ cos__q_LH_KFE__ = TRAIT::cos( q(LH_KFE));
+
+ (*this)(0,0) = cos__q_LH_KFE__;
+ (*this)(0,1) = sin__q_LH_KFE__;
+ (*this)(1,0) = - sin__q_LH_KFE__;
+ (*this)(1,1) = cos__q_LH_KFE__;
+ (*this)(3,2) = ( 0.35 * sin__q_LH_KFE__);
+ (*this)(3,3) = cos__q_LH_KFE__;
+ (*this)(3,4) = sin__q_LH_KFE__;
+ (*this)(4,2) = ( 0.35 * cos__q_LH_KFE__);
+ (*this)(4,3) = - sin__q_LH_KFE__;
+ (*this)(4,4) = cos__q_LH_KFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LH_upperleg_X_fr_LH_lowerleg::Type_fr_LH_upperleg_X_fr_LH_lowerleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = - 0.35;
+ (*this)(4,5) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LH_upperleg_X_fr_LH_lowerleg& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_LH_upperleg_X_fr_LH_lowerleg::update(const JState& q) {
+ Scalar sin__q_LH_KFE__;
+ Scalar cos__q_LH_KFE__;
+
+ sin__q_LH_KFE__ = TRAIT::sin( q(LH_KFE));
+ cos__q_LH_KFE__ = TRAIT::cos( q(LH_KFE));
+
+ (*this)(0,0) = cos__q_LH_KFE__;
+ (*this)(0,1) = - sin__q_LH_KFE__;
+ (*this)(1,0) = sin__q_LH_KFE__;
+ (*this)(1,1) = cos__q_LH_KFE__;
+ (*this)(3,3) = cos__q_LH_KFE__;
+ (*this)(3,4) = - sin__q_LH_KFE__;
+ (*this)(4,3) = sin__q_LH_KFE__;
+ (*this)(4,4) = cos__q_LH_KFE__;
+ (*this)(5,0) = ( 0.35 * sin__q_LH_KFE__);
+ (*this)(5,1) = ( 0.35 * cos__q_LH_KFE__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RH_upperleg_X_fr_RH_hipassembly::Type_fr_RH_upperleg_X_fr_RH_hipassembly()
+{
+ (*this)(0,1) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,1) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 1.0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,4) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,4) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0.08;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 1;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RH_upperleg_X_fr_RH_hipassembly& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RH_upperleg_X_fr_RH_hipassembly::update(const JState& q) {
+ Scalar sin__q_RH_HFE__;
+ Scalar cos__q_RH_HFE__;
+
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+
+ (*this)(0,0) = cos__q_RH_HFE__;
+ (*this)(0,2) = - sin__q_RH_HFE__;
+ (*this)(1,0) = - sin__q_RH_HFE__;
+ (*this)(1,2) = - cos__q_RH_HFE__;
+ (*this)(3,1) = ( 0.08 * sin__q_RH_HFE__);
+ (*this)(3,3) = cos__q_RH_HFE__;
+ (*this)(3,5) = - sin__q_RH_HFE__;
+ (*this)(4,1) = ( 0.08 * cos__q_RH_HFE__);
+ (*this)(4,3) = - sin__q_RH_HFE__;
+ (*this)(4,5) = - cos__q_RH_HFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RH_hipassembly_X_fr_RH_upperleg::Type_fr_RH_hipassembly_X_fr_RH_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 0;
+ (*this)(1,2) = 1;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = 0;
+ (*this)(4,5) = 1.0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0.08;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RH_hipassembly_X_fr_RH_upperleg& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RH_hipassembly_X_fr_RH_upperleg::update(const JState& q) {
+ Scalar sin__q_RH_HFE__;
+ Scalar cos__q_RH_HFE__;
+
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+
+ (*this)(0,0) = cos__q_RH_HFE__;
+ (*this)(0,1) = - sin__q_RH_HFE__;
+ (*this)(2,0) = - sin__q_RH_HFE__;
+ (*this)(2,1) = - cos__q_RH_HFE__;
+ (*this)(3,3) = cos__q_RH_HFE__;
+ (*this)(3,4) = - sin__q_RH_HFE__;
+ (*this)(4,0) = ( 0.08 * sin__q_RH_HFE__);
+ (*this)(4,1) = ( 0.08 * cos__q_RH_HFE__);
+ (*this)(5,3) = - sin__q_RH_HFE__;
+ (*this)(5,4) = - cos__q_RH_HFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RH_lowerleg_X_fr_RH_upperleg::Type_fr_RH_lowerleg_X_fr_RH_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1.0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = - 0.35;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RH_lowerleg_X_fr_RH_upperleg& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RH_lowerleg_X_fr_RH_upperleg::update(const JState& q) {
+ Scalar sin__q_RH_KFE__;
+ Scalar cos__q_RH_KFE__;
+
+ sin__q_RH_KFE__ = TRAIT::sin( q(RH_KFE));
+ cos__q_RH_KFE__ = TRAIT::cos( q(RH_KFE));
+
+ (*this)(0,0) = cos__q_RH_KFE__;
+ (*this)(0,1) = sin__q_RH_KFE__;
+ (*this)(1,0) = - sin__q_RH_KFE__;
+ (*this)(1,1) = cos__q_RH_KFE__;
+ (*this)(3,2) = ( 0.35 * sin__q_RH_KFE__);
+ (*this)(3,3) = cos__q_RH_KFE__;
+ (*this)(3,4) = sin__q_RH_KFE__;
+ (*this)(4,2) = ( 0.35 * cos__q_RH_KFE__);
+ (*this)(4,3) = - sin__q_RH_KFE__;
+ (*this)(4,4) = cos__q_RH_KFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RH_upperleg_X_fr_RH_lowerleg::Type_fr_RH_upperleg_X_fr_RH_lowerleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = - 0.35;
+ (*this)(4,5) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RH_upperleg_X_fr_RH_lowerleg& iit::TestHyQ::tpl::MotionTransforms<TRAIT>::Type_fr_RH_upperleg_X_fr_RH_lowerleg::update(const JState& q) {
+ Scalar sin__q_RH_KFE__;
+ Scalar cos__q_RH_KFE__;
+
+ sin__q_RH_KFE__ = TRAIT::sin( q(RH_KFE));
+ cos__q_RH_KFE__ = TRAIT::cos( q(RH_KFE));
+
+ (*this)(0,0) = cos__q_RH_KFE__;
+ (*this)(0,1) = - sin__q_RH_KFE__;
+ (*this)(1,0) = sin__q_RH_KFE__;
+ (*this)(1,1) = cos__q_RH_KFE__;
+ (*this)(3,3) = cos__q_RH_KFE__;
+ (*this)(3,4) = - sin__q_RH_KFE__;
+ (*this)(4,3) = sin__q_RH_KFE__;
+ (*this)(4,4) = cos__q_RH_KFE__;
+ (*this)(5,0) = ( 0.35 * sin__q_RH_KFE__);
+ (*this)(5,1) = ( 0.35 * cos__q_RH_KFE__);
+ return *this;
+}
+
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_hipassembly::Type_fr_trunk_X_fr_LF_hipassembly()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = - 1.0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,5) = 0.207;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = - 1.0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_hipassembly& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_hipassembly::update(const JState& q) {
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,3) = (- 0.207 * cos__q_LF_HAA__);
+ (*this)(0,4) = ( 0.207 * sin__q_LF_HAA__);
+ (*this)(1,0) = - sin__q_LF_HAA__;
+ (*this)(1,1) = - cos__q_LF_HAA__;
+ (*this)(1,3) = ( 0.3735 * cos__q_LF_HAA__);
+ (*this)(1,4) = (- 0.3735 * sin__q_LF_HAA__);
+ (*this)(2,0) = - cos__q_LF_HAA__;
+ (*this)(2,1) = sin__q_LF_HAA__;
+ (*this)(2,3) = (- 0.3735 * sin__q_LF_HAA__);
+ (*this)(2,4) = (- 0.3735 * cos__q_LF_HAA__);
+ (*this)(4,3) = - sin__q_LF_HAA__;
+ (*this)(4,4) = - cos__q_LF_HAA__;
+ (*this)(5,3) = - cos__q_LF_HAA__;
+ (*this)(5,4) = sin__q_LF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_hipassembly::Type_fr_trunk_X_fr_RF_hipassembly()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 1.0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,5) = 0.207;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 1.0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_hipassembly& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_hipassembly::update(const JState& q) {
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,3) = ( 0.207 * cos__q_RF_HAA__);
+ (*this)(0,4) = (- 0.207 * sin__q_RF_HAA__);
+ (*this)(1,0) = sin__q_RF_HAA__;
+ (*this)(1,1) = cos__q_RF_HAA__;
+ (*this)(1,3) = ( 0.3735 * cos__q_RF_HAA__);
+ (*this)(1,4) = (- 0.3735 * sin__q_RF_HAA__);
+ (*this)(2,0) = - cos__q_RF_HAA__;
+ (*this)(2,1) = sin__q_RF_HAA__;
+ (*this)(2,3) = ( 0.3735 * sin__q_RF_HAA__);
+ (*this)(2,4) = ( 0.3735 * cos__q_RF_HAA__);
+ (*this)(4,3) = sin__q_RF_HAA__;
+ (*this)(4,4) = cos__q_RF_HAA__;
+ (*this)(5,3) = - cos__q_RF_HAA__;
+ (*this)(5,4) = sin__q_RF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_hipassembly::Type_fr_trunk_X_fr_LH_hipassembly()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = - 1.0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,5) = 0.207;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = - 1.0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_hipassembly& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_hipassembly::update(const JState& q) {
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,3) = (- 0.207 * cos__q_LH_HAA__);
+ (*this)(0,4) = ( 0.207 * sin__q_LH_HAA__);
+ (*this)(1,0) = - sin__q_LH_HAA__;
+ (*this)(1,1) = - cos__q_LH_HAA__;
+ (*this)(1,3) = (- 0.3735 * cos__q_LH_HAA__);
+ (*this)(1,4) = ( 0.3735 * sin__q_LH_HAA__);
+ (*this)(2,0) = - cos__q_LH_HAA__;
+ (*this)(2,1) = sin__q_LH_HAA__;
+ (*this)(2,3) = ( 0.3735 * sin__q_LH_HAA__);
+ (*this)(2,4) = ( 0.3735 * cos__q_LH_HAA__);
+ (*this)(4,3) = - sin__q_LH_HAA__;
+ (*this)(4,4) = - cos__q_LH_HAA__;
+ (*this)(5,3) = - cos__q_LH_HAA__;
+ (*this)(5,4) = sin__q_LH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_hipassembly::Type_fr_trunk_X_fr_RH_hipassembly()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 1.0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,5) = 0.207;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 1.0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_hipassembly& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_hipassembly::update(const JState& q) {
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,3) = ( 0.207 * cos__q_RH_HAA__);
+ (*this)(0,4) = (- 0.207 * sin__q_RH_HAA__);
+ (*this)(1,0) = sin__q_RH_HAA__;
+ (*this)(1,1) = cos__q_RH_HAA__;
+ (*this)(1,3) = (- 0.3735 * cos__q_RH_HAA__);
+ (*this)(1,4) = ( 0.3735 * sin__q_RH_HAA__);
+ (*this)(2,0) = - cos__q_RH_HAA__;
+ (*this)(2,1) = sin__q_RH_HAA__;
+ (*this)(2,3) = (- 0.3735 * sin__q_RH_HAA__);
+ (*this)(2,4) = (- 0.3735 * cos__q_RH_HAA__);
+ (*this)(4,3) = sin__q_RH_HAA__;
+ (*this)(4,4) = cos__q_RH_HAA__;
+ (*this)(5,3) = - cos__q_RH_HAA__;
+ (*this)(5,4) = sin__q_RH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_upperleg::Type_fr_trunk_X_fr_LF_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_upperleg& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_upperleg::update(const JState& q) {
+ Scalar sin__q_LF_HFE__;
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,0) = - sin__q_LF_HFE__;
+ (*this)(0,1) = - cos__q_LF_HFE__;
+ (*this)(0,3) = ((- 0.207 * cos__q_LF_HAA__) * cos__q_LF_HFE__);
+ (*this)(0,4) = (( 0.207 * cos__q_LF_HAA__) * sin__q_LF_HFE__);
+ (*this)(0,5) = ( 0.08 - ( 0.207 * sin__q_LF_HAA__));
+ (*this)(1,0) = (- sin__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(1,1) = ( sin__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(1,2) = cos__q_LF_HAA__;
+ (*this)(1,3) = ((( 0.08 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.3735 * cos__q_LF_HAA__) * cos__q_LF_HFE__));
+ (*this)(1,4) = ((( 0.08 * cos__q_LF_HAA__) * cos__q_LF_HFE__) - (( 0.3735 * cos__q_LF_HAA__) * sin__q_LF_HFE__));
+ (*this)(1,5) = ( 0.3735 * sin__q_LF_HAA__);
+ (*this)(2,0) = (- cos__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(2,1) = ( cos__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(2,3) = ((( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * sin__q_LF_HFE__) - (( 0.3735 * sin__q_LF_HAA__) * cos__q_LF_HFE__));
+ (*this)(2,4) = ((( 0.3735 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * cos__q_LF_HFE__));
+ (*this)(2,5) = ( 0.3735 * cos__q_LF_HAA__);
+ (*this)(3,3) = - sin__q_LF_HFE__;
+ (*this)(3,4) = - cos__q_LF_HFE__;
+ (*this)(4,3) = (- sin__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(4,4) = ( sin__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(4,5) = cos__q_LF_HAA__;
+ (*this)(5,3) = (- cos__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(5,4) = ( cos__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(5,5) = - sin__q_LF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_upperleg::Type_fr_trunk_X_fr_RF_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_upperleg& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_upperleg::update(const JState& q) {
+ Scalar sin__q_RF_HFE__;
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,0) = - sin__q_RF_HFE__;
+ (*this)(0,1) = - cos__q_RF_HFE__;
+ (*this)(0,3) = (( 0.207 * cos__q_RF_HAA__) * cos__q_RF_HFE__);
+ (*this)(0,4) = ((- 0.207 * cos__q_RF_HAA__) * sin__q_RF_HFE__);
+ (*this)(0,5) = ( 0.08 - ( 0.207 * sin__q_RF_HAA__));
+ (*this)(1,0) = ( sin__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(1,1) = (- sin__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(1,2) = cos__q_RF_HAA__;
+ (*this)(1,3) = ((( 0.08 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.3735 * cos__q_RF_HAA__) * cos__q_RF_HFE__));
+ (*this)(1,4) = ((( 0.08 * cos__q_RF_HAA__) * cos__q_RF_HFE__) - (( 0.3735 * cos__q_RF_HAA__) * sin__q_RF_HFE__));
+ (*this)(1,5) = (- 0.3735 * sin__q_RF_HAA__);
+ (*this)(2,0) = (- cos__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(2,1) = ( cos__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(2,3) = (((( 0.08 * sin__q_RF_HAA__) - 0.207) * sin__q_RF_HFE__) + (( 0.3735 * sin__q_RF_HAA__) * cos__q_RF_HFE__));
+ (*this)(2,4) = (((( 0.08 * sin__q_RF_HAA__) - 0.207) * cos__q_RF_HFE__) - (( 0.3735 * sin__q_RF_HAA__) * sin__q_RF_HFE__));
+ (*this)(2,5) = ( 0.3735 * cos__q_RF_HAA__);
+ (*this)(3,3) = - sin__q_RF_HFE__;
+ (*this)(3,4) = - cos__q_RF_HFE__;
+ (*this)(4,3) = ( sin__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(4,4) = (- sin__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(4,5) = cos__q_RF_HAA__;
+ (*this)(5,3) = (- cos__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(5,4) = ( cos__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(5,5) = sin__q_RF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_upperleg::Type_fr_trunk_X_fr_LH_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_upperleg& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_upperleg::update(const JState& q) {
+ Scalar sin__q_LH_HFE__;
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,0) = - sin__q_LH_HFE__;
+ (*this)(0,1) = - cos__q_LH_HFE__;
+ (*this)(0,3) = ((- 0.207 * cos__q_LH_HAA__) * cos__q_LH_HFE__);
+ (*this)(0,4) = (( 0.207 * cos__q_LH_HAA__) * sin__q_LH_HFE__);
+ (*this)(0,5) = ( 0.08 - ( 0.207 * sin__q_LH_HAA__));
+ (*this)(1,0) = (- sin__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(1,1) = ( sin__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(1,2) = cos__q_LH_HAA__;
+ (*this)(1,3) = ((( 0.08 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.3735 * cos__q_LH_HAA__) * cos__q_LH_HFE__));
+ (*this)(1,4) = ((( 0.3735 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.08 * cos__q_LH_HAA__) * cos__q_LH_HFE__));
+ (*this)(1,5) = (- 0.3735 * sin__q_LH_HAA__);
+ (*this)(2,0) = (- cos__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(2,1) = ( cos__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(2,3) = ((( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * sin__q_LH_HFE__) + (( 0.3735 * sin__q_LH_HAA__) * cos__q_LH_HFE__));
+ (*this)(2,4) = ((( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * cos__q_LH_HFE__) - (( 0.3735 * sin__q_LH_HAA__) * sin__q_LH_HFE__));
+ (*this)(2,5) = (- 0.3735 * cos__q_LH_HAA__);
+ (*this)(3,3) = - sin__q_LH_HFE__;
+ (*this)(3,4) = - cos__q_LH_HFE__;
+ (*this)(4,3) = (- sin__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(4,4) = ( sin__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(4,5) = cos__q_LH_HAA__;
+ (*this)(5,3) = (- cos__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(5,4) = ( cos__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(5,5) = - sin__q_LH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_upperleg::Type_fr_trunk_X_fr_RH_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_upperleg& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_upperleg::update(const JState& q) {
+ Scalar sin__q_RH_HFE__;
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,0) = - sin__q_RH_HFE__;
+ (*this)(0,1) = - cos__q_RH_HFE__;
+ (*this)(0,3) = (( 0.207 * cos__q_RH_HAA__) * cos__q_RH_HFE__);
+ (*this)(0,4) = ((- 0.207 * cos__q_RH_HAA__) * sin__q_RH_HFE__);
+ (*this)(0,5) = ( 0.08 - ( 0.207 * sin__q_RH_HAA__));
+ (*this)(1,0) = ( sin__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(1,1) = (- sin__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(1,2) = cos__q_RH_HAA__;
+ (*this)(1,3) = ((( 0.08 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.3735 * cos__q_RH_HAA__) * cos__q_RH_HFE__));
+ (*this)(1,4) = ((( 0.3735 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.08 * cos__q_RH_HAA__) * cos__q_RH_HFE__));
+ (*this)(1,5) = ( 0.3735 * sin__q_RH_HAA__);
+ (*this)(2,0) = (- cos__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(2,1) = ( cos__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(2,3) = (((( 0.08 * sin__q_RH_HAA__) - 0.207) * sin__q_RH_HFE__) - (( 0.3735 * sin__q_RH_HAA__) * cos__q_RH_HFE__));
+ (*this)(2,4) = ((( 0.3735 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ((( 0.08 * sin__q_RH_HAA__) - 0.207) * cos__q_RH_HFE__));
+ (*this)(2,5) = (- 0.3735 * cos__q_RH_HAA__);
+ (*this)(3,3) = - sin__q_RH_HFE__;
+ (*this)(3,4) = - cos__q_RH_HFE__;
+ (*this)(4,3) = ( sin__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(4,4) = (- sin__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(4,5) = cos__q_RH_HAA__;
+ (*this)(5,3) = (- cos__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(5,4) = ( cos__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(5,5) = sin__q_RH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_lowerleg::Type_fr_trunk_X_fr_LF_lowerleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_lowerleg& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_lowerleg::update(const JState& q) {
+ Scalar sin__q_LF_KFE__;
+ Scalar sin__q_LF_HFE__;
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+ Scalar cos__q_LF_KFE__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_KFE__ = TRAIT::sin( q(LF_KFE));
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+ cos__q_LF_KFE__ = TRAIT::cos( q(LF_KFE));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,0) = ((- cos__q_LF_HFE__ * sin__q_LF_KFE__) - ( sin__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(0,1) = (( sin__q_LF_HFE__ * sin__q_LF_KFE__) - ( cos__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(0,3) = (((( 0.207 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * sin__q_LF_KFE__) - ((( 0.207 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(0,4) = (((( 0.207 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) + ((( 0.207 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(0,5) = ((( 0.35 * cos__q_LF_HFE__) - ( 0.207 * sin__q_LF_HAA__)) + 0.08);
+ (*this)(1,0) = ((( sin__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( sin__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,1) = ((( sin__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( sin__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,2) = cos__q_LF_HAA__;
+ (*this)(1,3) = ((((((- 0.3735 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.08 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.35 * cos__q_LF_HAA__)) * sin__q_LF_KFE__) + (((( 0.08 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.3735 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) * cos__q_LF_KFE__));
+ (*this)(1,4) = (((((- 0.08 * cos__q_LF_HAA__) * sin__q_LF_HFE__) - (( 0.3735 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) * sin__q_LF_KFE__) + (((((- 0.3735 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.08 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.35 * cos__q_LF_HAA__)) * cos__q_LF_KFE__));
+ (*this)(1,5) = (( 0.3735 * sin__q_LF_HAA__) - (( 0.35 * sin__q_LF_HAA__) * sin__q_LF_HFE__));
+ (*this)(2,0) = ((( cos__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( cos__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(2,1) = ((( cos__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( cos__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(2,3) = (((((( 0.3735 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * cos__q_LF_HFE__)) - ( 0.35 * sin__q_LF_HAA__)) * sin__q_LF_KFE__) + (((( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * sin__q_LF_HFE__) - (( 0.3735 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) * cos__q_LF_KFE__));
+ (*this)(2,4) = (((((( 0.08 * sin__q_LF_HAA__) - 0.207) * sin__q_LF_HFE__) + (( 0.3735 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) * sin__q_LF_KFE__) + ((((( 0.3735 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * cos__q_LF_HFE__)) - ( 0.35 * sin__q_LF_HAA__)) * cos__q_LF_KFE__));
+ (*this)(2,5) = (( 0.3735 * cos__q_LF_HAA__) - (( 0.35 * cos__q_LF_HAA__) * sin__q_LF_HFE__));
+ (*this)(3,3) = ((- cos__q_LF_HFE__ * sin__q_LF_KFE__) - ( sin__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(3,4) = (( sin__q_LF_HFE__ * sin__q_LF_KFE__) - ( cos__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(4,3) = ((( sin__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( sin__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(4,4) = ((( sin__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( sin__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(4,5) = cos__q_LF_HAA__;
+ (*this)(5,3) = ((( cos__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( cos__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(5,4) = ((( cos__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( cos__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(5,5) = - sin__q_LF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_lowerleg::Type_fr_trunk_X_fr_RF_lowerleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_lowerleg& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_lowerleg::update(const JState& q) {
+ Scalar sin__q_RF_KFE__;
+ Scalar sin__q_RF_HFE__;
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+ Scalar cos__q_RF_KFE__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_KFE__ = TRAIT::sin( q(RF_KFE));
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+ cos__q_RF_KFE__ = TRAIT::cos( q(RF_KFE));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,0) = ((- cos__q_RF_HFE__ * sin__q_RF_KFE__) - ( sin__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(0,1) = (( sin__q_RF_HFE__ * sin__q_RF_KFE__) - ( cos__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(0,3) = (((( 0.207 * cos__q_RF_HAA__) * cos__q_RF_HFE__) * cos__q_RF_KFE__) - ((( 0.207 * cos__q_RF_HAA__) * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(0,4) = ((((- 0.207 * cos__q_RF_HAA__) * cos__q_RF_HFE__) * sin__q_RF_KFE__) - ((( 0.207 * cos__q_RF_HAA__) * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(0,5) = ((( 0.35 * cos__q_RF_HFE__) - ( 0.207 * sin__q_RF_HAA__)) + 0.08);
+ (*this)(1,0) = ((( sin__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(1,1) = (((- sin__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(1,2) = cos__q_RF_HAA__;
+ (*this)(1,3) = ((((((- 0.3735 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.08 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.35 * cos__q_RF_HAA__)) * sin__q_RF_KFE__) + (((( 0.08 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.3735 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) * cos__q_RF_KFE__));
+ (*this)(1,4) = (((((- 0.08 * cos__q_RF_HAA__) * sin__q_RF_HFE__) - (( 0.3735 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + (((((- 0.3735 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.08 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.35 * cos__q_RF_HAA__)) * cos__q_RF_KFE__));
+ (*this)(1,5) = ((( 0.35 * sin__q_RF_HAA__) * sin__q_RF_HFE__) - ( 0.3735 * sin__q_RF_HAA__));
+ (*this)(2,0) = ((( cos__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__) - (( cos__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(2,1) = ((( cos__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) + (( cos__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(2,3) = ((((((- 0.3735 * sin__q_RF_HAA__) * sin__q_RF_HFE__) + ((( 0.08 * sin__q_RF_HAA__) - 0.207) * cos__q_RF_HFE__)) + ( 0.35 * sin__q_RF_HAA__)) * sin__q_RF_KFE__) + ((((( 0.08 * sin__q_RF_HAA__) - 0.207) * sin__q_RF_HFE__) + (( 0.3735 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) * cos__q_RF_KFE__));
+ (*this)(2,4) = ((((( 0.207 - ( 0.08 * sin__q_RF_HAA__)) * sin__q_RF_HFE__) - (( 0.3735 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + (((((- 0.3735 * sin__q_RF_HAA__) * sin__q_RF_HFE__) + ((( 0.08 * sin__q_RF_HAA__) - 0.207) * cos__q_RF_HFE__)) + ( 0.35 * sin__q_RF_HAA__)) * cos__q_RF_KFE__));
+ (*this)(2,5) = (( 0.3735 * cos__q_RF_HAA__) - (( 0.35 * cos__q_RF_HAA__) * sin__q_RF_HFE__));
+ (*this)(3,3) = ((- cos__q_RF_HFE__ * sin__q_RF_KFE__) - ( sin__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(3,4) = (( sin__q_RF_HFE__ * sin__q_RF_KFE__) - ( cos__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(4,3) = ((( sin__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(4,4) = (((- sin__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(4,5) = cos__q_RF_HAA__;
+ (*this)(5,3) = ((( cos__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__) - (( cos__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(5,4) = ((( cos__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) + (( cos__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(5,5) = sin__q_RF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_lowerleg::Type_fr_trunk_X_fr_LH_lowerleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_lowerleg& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_lowerleg::update(const JState& q) {
+ Scalar sin__q_LH_KFE__;
+ Scalar sin__q_LH_HFE__;
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+ Scalar cos__q_LH_KFE__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_KFE__ = TRAIT::sin( q(LH_KFE));
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+ cos__q_LH_KFE__ = TRAIT::cos( q(LH_KFE));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,0) = ((- cos__q_LH_HFE__ * sin__q_LH_KFE__) - ( sin__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(0,1) = (( sin__q_LH_HFE__ * sin__q_LH_KFE__) - ( cos__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(0,3) = (((( 0.207 * cos__q_LH_HAA__) * sin__q_LH_HFE__) * sin__q_LH_KFE__) - ((( 0.207 * cos__q_LH_HAA__) * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(0,4) = (((( 0.207 * cos__q_LH_HAA__) * cos__q_LH_HFE__) * sin__q_LH_KFE__) + ((( 0.207 * cos__q_LH_HAA__) * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(0,5) = ((( 0.35 * cos__q_LH_HFE__) - ( 0.207 * sin__q_LH_HAA__)) + 0.08);
+ (*this)(1,0) = ((( sin__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( sin__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,1) = ((( sin__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( sin__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,2) = cos__q_LH_HAA__;
+ (*this)(1,3) = (((((( 0.3735 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.08 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) + ( 0.35 * cos__q_LH_HAA__)) * sin__q_LH_KFE__) + (((( 0.08 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.3735 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) * cos__q_LH_KFE__));
+ (*this)(1,4) = ((((( 0.3735 * cos__q_LH_HAA__) * cos__q_LH_HFE__) - (( 0.08 * cos__q_LH_HAA__) * sin__q_LH_HFE__)) * sin__q_LH_KFE__) + ((((( 0.3735 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.08 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) + ( 0.35 * cos__q_LH_HAA__)) * cos__q_LH_KFE__));
+ (*this)(1,5) = (((- 0.35 * sin__q_LH_HAA__) * sin__q_LH_HFE__) - ( 0.3735 * sin__q_LH_HAA__));
+ (*this)(2,0) = ((( cos__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( cos__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(2,1) = ((( cos__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( cos__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(2,3) = ((((((- 0.3735 * sin__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * cos__q_LH_HFE__)) - ( 0.35 * sin__q_LH_HAA__)) * sin__q_LH_KFE__) + (((( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * sin__q_LH_HFE__) + (( 0.3735 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) * cos__q_LH_KFE__));
+ (*this)(2,4) = (((((( 0.08 * sin__q_LH_HAA__) - 0.207) * sin__q_LH_HFE__) - (( 0.3735 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) * sin__q_LH_KFE__) + (((((- 0.3735 * sin__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * cos__q_LH_HFE__)) - ( 0.35 * sin__q_LH_HAA__)) * cos__q_LH_KFE__));
+ (*this)(2,5) = (((- 0.35 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - ( 0.3735 * cos__q_LH_HAA__));
+ (*this)(3,3) = ((- cos__q_LH_HFE__ * sin__q_LH_KFE__) - ( sin__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(3,4) = (( sin__q_LH_HFE__ * sin__q_LH_KFE__) - ( cos__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(4,3) = ((( sin__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( sin__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(4,4) = ((( sin__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( sin__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(4,5) = cos__q_LH_HAA__;
+ (*this)(5,3) = ((( cos__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( cos__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(5,4) = ((( cos__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( cos__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(5,5) = - sin__q_LH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_lowerleg::Type_fr_trunk_X_fr_RH_lowerleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_lowerleg& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_lowerleg::update(const JState& q) {
+ Scalar sin__q_RH_KFE__;
+ Scalar sin__q_RH_HFE__;
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+ Scalar cos__q_RH_KFE__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_KFE__ = TRAIT::sin( q(RH_KFE));
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+ cos__q_RH_KFE__ = TRAIT::cos( q(RH_KFE));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,0) = ((- cos__q_RH_HFE__ * sin__q_RH_KFE__) - ( sin__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(0,1) = (( sin__q_RH_HFE__ * sin__q_RH_KFE__) - ( cos__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(0,3) = (((( 0.207 * cos__q_RH_HAA__) * cos__q_RH_HFE__) * cos__q_RH_KFE__) - ((( 0.207 * cos__q_RH_HAA__) * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(0,4) = ((((- 0.207 * cos__q_RH_HAA__) * cos__q_RH_HFE__) * sin__q_RH_KFE__) - ((( 0.207 * cos__q_RH_HAA__) * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(0,5) = ((( 0.35 * cos__q_RH_HFE__) - ( 0.207 * sin__q_RH_HAA__)) + 0.08);
+ (*this)(1,0) = ((( sin__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(1,1) = (((- sin__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(1,2) = cos__q_RH_HAA__;
+ (*this)(1,3) = (((((( 0.3735 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.08 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.35 * cos__q_RH_HAA__)) * sin__q_RH_KFE__) + (((( 0.08 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.3735 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) * cos__q_RH_KFE__));
+ (*this)(1,4) = ((((( 0.3735 * cos__q_RH_HAA__) * cos__q_RH_HFE__) - (( 0.08 * cos__q_RH_HAA__) * sin__q_RH_HFE__)) * sin__q_RH_KFE__) + ((((( 0.3735 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.08 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.35 * cos__q_RH_HAA__)) * cos__q_RH_KFE__));
+ (*this)(1,5) = ((( 0.35 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ( 0.3735 * sin__q_RH_HAA__));
+ (*this)(2,0) = ((( cos__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__) - (( cos__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(2,1) = ((( cos__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) + (( cos__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(2,3) = (((((( 0.3735 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ((( 0.08 * sin__q_RH_HAA__) - 0.207) * cos__q_RH_HFE__)) + ( 0.35 * sin__q_RH_HAA__)) * sin__q_RH_KFE__) + ((((( 0.08 * sin__q_RH_HAA__) - 0.207) * sin__q_RH_HFE__) - (( 0.3735 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) * cos__q_RH_KFE__));
+ (*this)(2,4) = ((((( 0.207 - ( 0.08 * sin__q_RH_HAA__)) * sin__q_RH_HFE__) + (( 0.3735 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) * sin__q_RH_KFE__) + ((((( 0.3735 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ((( 0.08 * sin__q_RH_HAA__) - 0.207) * cos__q_RH_HFE__)) + ( 0.35 * sin__q_RH_HAA__)) * cos__q_RH_KFE__));
+ (*this)(2,5) = (((- 0.35 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - ( 0.3735 * cos__q_RH_HAA__));
+ (*this)(3,3) = ((- cos__q_RH_HFE__ * sin__q_RH_KFE__) - ( sin__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(3,4) = (( sin__q_RH_HFE__ * sin__q_RH_KFE__) - ( cos__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(4,3) = ((( sin__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(4,4) = (((- sin__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(4,5) = cos__q_RH_HAA__;
+ (*this)(5,3) = ((( cos__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__) - (( cos__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(5,4) = ((( cos__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) + (( cos__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(5,5) = sin__q_RH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LF_hipassembly_X_fr_trunk::Type_fr_LF_hipassembly_X_fr_trunk()
+{
+ (*this)(0,0) = 0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = - 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0.207;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = - 1.0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LF_hipassembly_X_fr_trunk& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LF_hipassembly_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,1) = - sin__q_LF_HAA__;
+ (*this)(0,2) = - cos__q_LF_HAA__;
+ (*this)(0,3) = (- 0.207 * cos__q_LF_HAA__);
+ (*this)(0,4) = ( 0.3735 * cos__q_LF_HAA__);
+ (*this)(0,5) = (- 0.3735 * sin__q_LF_HAA__);
+ (*this)(1,1) = - cos__q_LF_HAA__;
+ (*this)(1,2) = sin__q_LF_HAA__;
+ (*this)(1,3) = ( 0.207 * sin__q_LF_HAA__);
+ (*this)(1,4) = (- 0.3735 * sin__q_LF_HAA__);
+ (*this)(1,5) = (- 0.3735 * cos__q_LF_HAA__);
+ (*this)(3,4) = - sin__q_LF_HAA__;
+ (*this)(3,5) = - cos__q_LF_HAA__;
+ (*this)(4,4) = - cos__q_LF_HAA__;
+ (*this)(4,5) = sin__q_LF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RF_hipassembly_X_fr_trunk::Type_fr_RF_hipassembly_X_fr_trunk()
+{
+ (*this)(0,0) = 0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0.207;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 1.0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RF_hipassembly_X_fr_trunk& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RF_hipassembly_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,1) = sin__q_RF_HAA__;
+ (*this)(0,2) = - cos__q_RF_HAA__;
+ (*this)(0,3) = ( 0.207 * cos__q_RF_HAA__);
+ (*this)(0,4) = ( 0.3735 * cos__q_RF_HAA__);
+ (*this)(0,5) = ( 0.3735 * sin__q_RF_HAA__);
+ (*this)(1,1) = cos__q_RF_HAA__;
+ (*this)(1,2) = sin__q_RF_HAA__;
+ (*this)(1,3) = (- 0.207 * sin__q_RF_HAA__);
+ (*this)(1,4) = (- 0.3735 * sin__q_RF_HAA__);
+ (*this)(1,5) = ( 0.3735 * cos__q_RF_HAA__);
+ (*this)(3,4) = sin__q_RF_HAA__;
+ (*this)(3,5) = - cos__q_RF_HAA__;
+ (*this)(4,4) = cos__q_RF_HAA__;
+ (*this)(4,5) = sin__q_RF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LH_hipassembly_X_fr_trunk::Type_fr_LH_hipassembly_X_fr_trunk()
+{
+ (*this)(0,0) = 0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = - 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0.207;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = - 1.0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LH_hipassembly_X_fr_trunk& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LH_hipassembly_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,1) = - sin__q_LH_HAA__;
+ (*this)(0,2) = - cos__q_LH_HAA__;
+ (*this)(0,3) = (- 0.207 * cos__q_LH_HAA__);
+ (*this)(0,4) = (- 0.3735 * cos__q_LH_HAA__);
+ (*this)(0,5) = ( 0.3735 * sin__q_LH_HAA__);
+ (*this)(1,1) = - cos__q_LH_HAA__;
+ (*this)(1,2) = sin__q_LH_HAA__;
+ (*this)(1,3) = ( 0.207 * sin__q_LH_HAA__);
+ (*this)(1,4) = ( 0.3735 * sin__q_LH_HAA__);
+ (*this)(1,5) = ( 0.3735 * cos__q_LH_HAA__);
+ (*this)(3,4) = - sin__q_LH_HAA__;
+ (*this)(3,5) = - cos__q_LH_HAA__;
+ (*this)(4,4) = - cos__q_LH_HAA__;
+ (*this)(4,5) = sin__q_LH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RH_hipassembly_X_fr_trunk::Type_fr_RH_hipassembly_X_fr_trunk()
+{
+ (*this)(0,0) = 0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0.207;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 1.0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RH_hipassembly_X_fr_trunk& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RH_hipassembly_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,1) = sin__q_RH_HAA__;
+ (*this)(0,2) = - cos__q_RH_HAA__;
+ (*this)(0,3) = ( 0.207 * cos__q_RH_HAA__);
+ (*this)(0,4) = (- 0.3735 * cos__q_RH_HAA__);
+ (*this)(0,5) = (- 0.3735 * sin__q_RH_HAA__);
+ (*this)(1,1) = cos__q_RH_HAA__;
+ (*this)(1,2) = sin__q_RH_HAA__;
+ (*this)(1,3) = (- 0.207 * sin__q_RH_HAA__);
+ (*this)(1,4) = ( 0.3735 * sin__q_RH_HAA__);
+ (*this)(1,5) = (- 0.3735 * cos__q_RH_HAA__);
+ (*this)(3,4) = sin__q_RH_HAA__;
+ (*this)(3,5) = - cos__q_RH_HAA__;
+ (*this)(4,4) = cos__q_RH_HAA__;
+ (*this)(4,5) = sin__q_RH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LF_upperleg_X_fr_trunk::Type_fr_LF_upperleg_X_fr_trunk()
+{
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LF_upperleg_X_fr_trunk& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LF_upperleg_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_LF_HFE__;
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,0) = - sin__q_LF_HFE__;
+ (*this)(0,1) = (- sin__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(0,2) = (- cos__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(0,3) = ((- 0.207 * cos__q_LF_HAA__) * cos__q_LF_HFE__);
+ (*this)(0,4) = ((( 0.08 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.3735 * cos__q_LF_HAA__) * cos__q_LF_HFE__));
+ (*this)(0,5) = ((( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * sin__q_LF_HFE__) - (( 0.3735 * sin__q_LF_HAA__) * cos__q_LF_HFE__));
+ (*this)(1,0) = - cos__q_LF_HFE__;
+ (*this)(1,1) = ( sin__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(1,2) = ( cos__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(1,3) = (( 0.207 * cos__q_LF_HAA__) * sin__q_LF_HFE__);
+ (*this)(1,4) = ((( 0.08 * cos__q_LF_HAA__) * cos__q_LF_HFE__) - (( 0.3735 * cos__q_LF_HAA__) * sin__q_LF_HFE__));
+ (*this)(1,5) = ((( 0.3735 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * cos__q_LF_HFE__));
+ (*this)(2,1) = cos__q_LF_HAA__;
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(2,3) = ( 0.08 - ( 0.207 * sin__q_LF_HAA__));
+ (*this)(2,4) = ( 0.3735 * sin__q_LF_HAA__);
+ (*this)(2,5) = ( 0.3735 * cos__q_LF_HAA__);
+ (*this)(3,3) = - sin__q_LF_HFE__;
+ (*this)(3,4) = (- sin__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(3,5) = (- cos__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(4,3) = - cos__q_LF_HFE__;
+ (*this)(4,4) = ( sin__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(4,5) = ( cos__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(5,4) = cos__q_LF_HAA__;
+ (*this)(5,5) = - sin__q_LF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RF_upperleg_X_fr_trunk::Type_fr_RF_upperleg_X_fr_trunk()
+{
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RF_upperleg_X_fr_trunk& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RF_upperleg_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_RF_HFE__;
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,0) = - sin__q_RF_HFE__;
+ (*this)(0,1) = ( sin__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(0,2) = (- cos__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(0,3) = (( 0.207 * cos__q_RF_HAA__) * cos__q_RF_HFE__);
+ (*this)(0,4) = ((( 0.08 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.3735 * cos__q_RF_HAA__) * cos__q_RF_HFE__));
+ (*this)(0,5) = (((( 0.08 * sin__q_RF_HAA__) - 0.207) * sin__q_RF_HFE__) + (( 0.3735 * sin__q_RF_HAA__) * cos__q_RF_HFE__));
+ (*this)(1,0) = - cos__q_RF_HFE__;
+ (*this)(1,1) = (- sin__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(1,2) = ( cos__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(1,3) = ((- 0.207 * cos__q_RF_HAA__) * sin__q_RF_HFE__);
+ (*this)(1,4) = ((( 0.08 * cos__q_RF_HAA__) * cos__q_RF_HFE__) - (( 0.3735 * cos__q_RF_HAA__) * sin__q_RF_HFE__));
+ (*this)(1,5) = (((( 0.08 * sin__q_RF_HAA__) - 0.207) * cos__q_RF_HFE__) - (( 0.3735 * sin__q_RF_HAA__) * sin__q_RF_HFE__));
+ (*this)(2,1) = cos__q_RF_HAA__;
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(2,3) = ( 0.08 - ( 0.207 * sin__q_RF_HAA__));
+ (*this)(2,4) = (- 0.3735 * sin__q_RF_HAA__);
+ (*this)(2,5) = ( 0.3735 * cos__q_RF_HAA__);
+ (*this)(3,3) = - sin__q_RF_HFE__;
+ (*this)(3,4) = ( sin__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(3,5) = (- cos__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(4,3) = - cos__q_RF_HFE__;
+ (*this)(4,4) = (- sin__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(4,5) = ( cos__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(5,4) = cos__q_RF_HAA__;
+ (*this)(5,5) = sin__q_RF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LH_upperleg_X_fr_trunk::Type_fr_LH_upperleg_X_fr_trunk()
+{
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LH_upperleg_X_fr_trunk& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LH_upperleg_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_LH_HFE__;
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,0) = - sin__q_LH_HFE__;
+ (*this)(0,1) = (- sin__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(0,2) = (- cos__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(0,3) = ((- 0.207 * cos__q_LH_HAA__) * cos__q_LH_HFE__);
+ (*this)(0,4) = ((( 0.08 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.3735 * cos__q_LH_HAA__) * cos__q_LH_HFE__));
+ (*this)(0,5) = ((( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * sin__q_LH_HFE__) + (( 0.3735 * sin__q_LH_HAA__) * cos__q_LH_HFE__));
+ (*this)(1,0) = - cos__q_LH_HFE__;
+ (*this)(1,1) = ( sin__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(1,2) = ( cos__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(1,3) = (( 0.207 * cos__q_LH_HAA__) * sin__q_LH_HFE__);
+ (*this)(1,4) = ((( 0.3735 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.08 * cos__q_LH_HAA__) * cos__q_LH_HFE__));
+ (*this)(1,5) = ((( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * cos__q_LH_HFE__) - (( 0.3735 * sin__q_LH_HAA__) * sin__q_LH_HFE__));
+ (*this)(2,1) = cos__q_LH_HAA__;
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(2,3) = ( 0.08 - ( 0.207 * sin__q_LH_HAA__));
+ (*this)(2,4) = (- 0.3735 * sin__q_LH_HAA__);
+ (*this)(2,5) = (- 0.3735 * cos__q_LH_HAA__);
+ (*this)(3,3) = - sin__q_LH_HFE__;
+ (*this)(3,4) = (- sin__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(3,5) = (- cos__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(4,3) = - cos__q_LH_HFE__;
+ (*this)(4,4) = ( sin__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(4,5) = ( cos__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(5,4) = cos__q_LH_HAA__;
+ (*this)(5,5) = - sin__q_LH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RH_upperleg_X_fr_trunk::Type_fr_RH_upperleg_X_fr_trunk()
+{
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RH_upperleg_X_fr_trunk& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RH_upperleg_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_RH_HFE__;
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,0) = - sin__q_RH_HFE__;
+ (*this)(0,1) = ( sin__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(0,2) = (- cos__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(0,3) = (( 0.207 * cos__q_RH_HAA__) * cos__q_RH_HFE__);
+ (*this)(0,4) = ((( 0.08 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.3735 * cos__q_RH_HAA__) * cos__q_RH_HFE__));
+ (*this)(0,5) = (((( 0.08 * sin__q_RH_HAA__) - 0.207) * sin__q_RH_HFE__) - (( 0.3735 * sin__q_RH_HAA__) * cos__q_RH_HFE__));
+ (*this)(1,0) = - cos__q_RH_HFE__;
+ (*this)(1,1) = (- sin__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(1,2) = ( cos__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(1,3) = ((- 0.207 * cos__q_RH_HAA__) * sin__q_RH_HFE__);
+ (*this)(1,4) = ((( 0.3735 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.08 * cos__q_RH_HAA__) * cos__q_RH_HFE__));
+ (*this)(1,5) = ((( 0.3735 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ((( 0.08 * sin__q_RH_HAA__) - 0.207) * cos__q_RH_HFE__));
+ (*this)(2,1) = cos__q_RH_HAA__;
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(2,3) = ( 0.08 - ( 0.207 * sin__q_RH_HAA__));
+ (*this)(2,4) = ( 0.3735 * sin__q_RH_HAA__);
+ (*this)(2,5) = (- 0.3735 * cos__q_RH_HAA__);
+ (*this)(3,3) = - sin__q_RH_HFE__;
+ (*this)(3,4) = ( sin__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(3,5) = (- cos__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(4,3) = - cos__q_RH_HFE__;
+ (*this)(4,4) = (- sin__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(4,5) = ( cos__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(5,4) = cos__q_RH_HAA__;
+ (*this)(5,5) = sin__q_RH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LF_lowerleg_X_fr_trunk::Type_fr_LF_lowerleg_X_fr_trunk()
+{
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LF_lowerleg_X_fr_trunk& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LF_lowerleg_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_LF_KFE__;
+ Scalar sin__q_LF_HFE__;
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+ Scalar cos__q_LF_KFE__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_KFE__ = TRAIT::sin( q(LF_KFE));
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+ cos__q_LF_KFE__ = TRAIT::cos( q(LF_KFE));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,0) = ((- cos__q_LF_HFE__ * sin__q_LF_KFE__) - ( sin__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(0,1) = ((( sin__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( sin__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(0,2) = ((( cos__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( cos__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(0,3) = (((( 0.207 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * sin__q_LF_KFE__) - ((( 0.207 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(0,4) = ((((((- 0.3735 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.08 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.35 * cos__q_LF_HAA__)) * sin__q_LF_KFE__) + (((( 0.08 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.3735 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) * cos__q_LF_KFE__));
+ (*this)(0,5) = (((((( 0.3735 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * cos__q_LF_HFE__)) - ( 0.35 * sin__q_LF_HAA__)) * sin__q_LF_KFE__) + (((( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * sin__q_LF_HFE__) - (( 0.3735 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) * cos__q_LF_KFE__));
+ (*this)(1,0) = (( sin__q_LF_HFE__ * sin__q_LF_KFE__) - ( cos__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(1,1) = ((( sin__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( sin__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,2) = ((( cos__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( cos__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,3) = (((( 0.207 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) + ((( 0.207 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,4) = (((((- 0.08 * cos__q_LF_HAA__) * sin__q_LF_HFE__) - (( 0.3735 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) * sin__q_LF_KFE__) + (((((- 0.3735 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.08 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.35 * cos__q_LF_HAA__)) * cos__q_LF_KFE__));
+ (*this)(1,5) = (((((( 0.08 * sin__q_LF_HAA__) - 0.207) * sin__q_LF_HFE__) + (( 0.3735 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) * sin__q_LF_KFE__) + ((((( 0.3735 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * cos__q_LF_HFE__)) - ( 0.35 * sin__q_LF_HAA__)) * cos__q_LF_KFE__));
+ (*this)(2,1) = cos__q_LF_HAA__;
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(2,3) = ((( 0.35 * cos__q_LF_HFE__) - ( 0.207 * sin__q_LF_HAA__)) + 0.08);
+ (*this)(2,4) = (( 0.3735 * sin__q_LF_HAA__) - (( 0.35 * sin__q_LF_HAA__) * sin__q_LF_HFE__));
+ (*this)(2,5) = (( 0.3735 * cos__q_LF_HAA__) - (( 0.35 * cos__q_LF_HAA__) * sin__q_LF_HFE__));
+ (*this)(3,3) = ((- cos__q_LF_HFE__ * sin__q_LF_KFE__) - ( sin__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(3,4) = ((( sin__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( sin__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(3,5) = ((( cos__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( cos__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(4,3) = (( sin__q_LF_HFE__ * sin__q_LF_KFE__) - ( cos__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(4,4) = ((( sin__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( sin__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(4,5) = ((( cos__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( cos__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(5,4) = cos__q_LF_HAA__;
+ (*this)(5,5) = - sin__q_LF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RF_lowerleg_X_fr_trunk::Type_fr_RF_lowerleg_X_fr_trunk()
+{
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RF_lowerleg_X_fr_trunk& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RF_lowerleg_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_RF_KFE__;
+ Scalar sin__q_RF_HFE__;
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+ Scalar cos__q_RF_KFE__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_KFE__ = TRAIT::sin( q(RF_KFE));
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+ cos__q_RF_KFE__ = TRAIT::cos( q(RF_KFE));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,0) = ((- cos__q_RF_HFE__ * sin__q_RF_KFE__) - ( sin__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(0,1) = ((( sin__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(0,2) = ((( cos__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__) - (( cos__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(0,3) = (((( 0.207 * cos__q_RF_HAA__) * cos__q_RF_HFE__) * cos__q_RF_KFE__) - ((( 0.207 * cos__q_RF_HAA__) * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(0,4) = ((((((- 0.3735 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.08 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.35 * cos__q_RF_HAA__)) * sin__q_RF_KFE__) + (((( 0.08 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.3735 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) * cos__q_RF_KFE__));
+ (*this)(0,5) = ((((((- 0.3735 * sin__q_RF_HAA__) * sin__q_RF_HFE__) + ((( 0.08 * sin__q_RF_HAA__) - 0.207) * cos__q_RF_HFE__)) + ( 0.35 * sin__q_RF_HAA__)) * sin__q_RF_KFE__) + ((((( 0.08 * sin__q_RF_HAA__) - 0.207) * sin__q_RF_HFE__) + (( 0.3735 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) * cos__q_RF_KFE__));
+ (*this)(1,0) = (( sin__q_RF_HFE__ * sin__q_RF_KFE__) - ( cos__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(1,1) = (((- sin__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(1,2) = ((( cos__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) + (( cos__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(1,3) = ((((- 0.207 * cos__q_RF_HAA__) * cos__q_RF_HFE__) * sin__q_RF_KFE__) - ((( 0.207 * cos__q_RF_HAA__) * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(1,4) = (((((- 0.08 * cos__q_RF_HAA__) * sin__q_RF_HFE__) - (( 0.3735 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + (((((- 0.3735 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.08 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.35 * cos__q_RF_HAA__)) * cos__q_RF_KFE__));
+ (*this)(1,5) = ((((( 0.207 - ( 0.08 * sin__q_RF_HAA__)) * sin__q_RF_HFE__) - (( 0.3735 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + (((((- 0.3735 * sin__q_RF_HAA__) * sin__q_RF_HFE__) + ((( 0.08 * sin__q_RF_HAA__) - 0.207) * cos__q_RF_HFE__)) + ( 0.35 * sin__q_RF_HAA__)) * cos__q_RF_KFE__));
+ (*this)(2,1) = cos__q_RF_HAA__;
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(2,3) = ((( 0.35 * cos__q_RF_HFE__) - ( 0.207 * sin__q_RF_HAA__)) + 0.08);
+ (*this)(2,4) = ((( 0.35 * sin__q_RF_HAA__) * sin__q_RF_HFE__) - ( 0.3735 * sin__q_RF_HAA__));
+ (*this)(2,5) = (( 0.3735 * cos__q_RF_HAA__) - (( 0.35 * cos__q_RF_HAA__) * sin__q_RF_HFE__));
+ (*this)(3,3) = ((- cos__q_RF_HFE__ * sin__q_RF_KFE__) - ( sin__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(3,4) = ((( sin__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(3,5) = ((( cos__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__) - (( cos__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(4,3) = (( sin__q_RF_HFE__ * sin__q_RF_KFE__) - ( cos__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(4,4) = (((- sin__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(4,5) = ((( cos__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) + (( cos__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(5,4) = cos__q_RF_HAA__;
+ (*this)(5,5) = sin__q_RF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LH_lowerleg_X_fr_trunk::Type_fr_LH_lowerleg_X_fr_trunk()
+{
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LH_lowerleg_X_fr_trunk& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LH_lowerleg_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_LH_KFE__;
+ Scalar sin__q_LH_HFE__;
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+ Scalar cos__q_LH_KFE__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_KFE__ = TRAIT::sin( q(LH_KFE));
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+ cos__q_LH_KFE__ = TRAIT::cos( q(LH_KFE));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,0) = ((- cos__q_LH_HFE__ * sin__q_LH_KFE__) - ( sin__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(0,1) = ((( sin__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( sin__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(0,2) = ((( cos__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( cos__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(0,3) = (((( 0.207 * cos__q_LH_HAA__) * sin__q_LH_HFE__) * sin__q_LH_KFE__) - ((( 0.207 * cos__q_LH_HAA__) * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(0,4) = (((((( 0.3735 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.08 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) + ( 0.35 * cos__q_LH_HAA__)) * sin__q_LH_KFE__) + (((( 0.08 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.3735 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) * cos__q_LH_KFE__));
+ (*this)(0,5) = ((((((- 0.3735 * sin__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * cos__q_LH_HFE__)) - ( 0.35 * sin__q_LH_HAA__)) * sin__q_LH_KFE__) + (((( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * sin__q_LH_HFE__) + (( 0.3735 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) * cos__q_LH_KFE__));
+ (*this)(1,0) = (( sin__q_LH_HFE__ * sin__q_LH_KFE__) - ( cos__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(1,1) = ((( sin__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( sin__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,2) = ((( cos__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( cos__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,3) = (((( 0.207 * cos__q_LH_HAA__) * cos__q_LH_HFE__) * sin__q_LH_KFE__) + ((( 0.207 * cos__q_LH_HAA__) * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,4) = ((((( 0.3735 * cos__q_LH_HAA__) * cos__q_LH_HFE__) - (( 0.08 * cos__q_LH_HAA__) * sin__q_LH_HFE__)) * sin__q_LH_KFE__) + ((((( 0.3735 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.08 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) + ( 0.35 * cos__q_LH_HAA__)) * cos__q_LH_KFE__));
+ (*this)(1,5) = (((((( 0.08 * sin__q_LH_HAA__) - 0.207) * sin__q_LH_HFE__) - (( 0.3735 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) * sin__q_LH_KFE__) + (((((- 0.3735 * sin__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * cos__q_LH_HFE__)) - ( 0.35 * sin__q_LH_HAA__)) * cos__q_LH_KFE__));
+ (*this)(2,1) = cos__q_LH_HAA__;
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(2,3) = ((( 0.35 * cos__q_LH_HFE__) - ( 0.207 * sin__q_LH_HAA__)) + 0.08);
+ (*this)(2,4) = (((- 0.35 * sin__q_LH_HAA__) * sin__q_LH_HFE__) - ( 0.3735 * sin__q_LH_HAA__));
+ (*this)(2,5) = (((- 0.35 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - ( 0.3735 * cos__q_LH_HAA__));
+ (*this)(3,3) = ((- cos__q_LH_HFE__ * sin__q_LH_KFE__) - ( sin__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(3,4) = ((( sin__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( sin__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(3,5) = ((( cos__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( cos__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(4,3) = (( sin__q_LH_HFE__ * sin__q_LH_KFE__) - ( cos__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(4,4) = ((( sin__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( sin__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(4,5) = ((( cos__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( cos__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(5,4) = cos__q_LH_HAA__;
+ (*this)(5,5) = - sin__q_LH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RH_lowerleg_X_fr_trunk::Type_fr_RH_lowerleg_X_fr_trunk()
+{
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RH_lowerleg_X_fr_trunk& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RH_lowerleg_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_RH_KFE__;
+ Scalar sin__q_RH_HFE__;
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+ Scalar cos__q_RH_KFE__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_KFE__ = TRAIT::sin( q(RH_KFE));
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+ cos__q_RH_KFE__ = TRAIT::cos( q(RH_KFE));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,0) = ((- cos__q_RH_HFE__ * sin__q_RH_KFE__) - ( sin__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(0,1) = ((( sin__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(0,2) = ((( cos__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__) - (( cos__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(0,3) = (((( 0.207 * cos__q_RH_HAA__) * cos__q_RH_HFE__) * cos__q_RH_KFE__) - ((( 0.207 * cos__q_RH_HAA__) * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(0,4) = (((((( 0.3735 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.08 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.35 * cos__q_RH_HAA__)) * sin__q_RH_KFE__) + (((( 0.08 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.3735 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) * cos__q_RH_KFE__));
+ (*this)(0,5) = (((((( 0.3735 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ((( 0.08 * sin__q_RH_HAA__) - 0.207) * cos__q_RH_HFE__)) + ( 0.35 * sin__q_RH_HAA__)) * sin__q_RH_KFE__) + ((((( 0.08 * sin__q_RH_HAA__) - 0.207) * sin__q_RH_HFE__) - (( 0.3735 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) * cos__q_RH_KFE__));
+ (*this)(1,0) = (( sin__q_RH_HFE__ * sin__q_RH_KFE__) - ( cos__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(1,1) = (((- sin__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(1,2) = ((( cos__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) + (( cos__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(1,3) = ((((- 0.207 * cos__q_RH_HAA__) * cos__q_RH_HFE__) * sin__q_RH_KFE__) - ((( 0.207 * cos__q_RH_HAA__) * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(1,4) = ((((( 0.3735 * cos__q_RH_HAA__) * cos__q_RH_HFE__) - (( 0.08 * cos__q_RH_HAA__) * sin__q_RH_HFE__)) * sin__q_RH_KFE__) + ((((( 0.3735 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.08 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.35 * cos__q_RH_HAA__)) * cos__q_RH_KFE__));
+ (*this)(1,5) = ((((( 0.207 - ( 0.08 * sin__q_RH_HAA__)) * sin__q_RH_HFE__) + (( 0.3735 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) * sin__q_RH_KFE__) + ((((( 0.3735 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ((( 0.08 * sin__q_RH_HAA__) - 0.207) * cos__q_RH_HFE__)) + ( 0.35 * sin__q_RH_HAA__)) * cos__q_RH_KFE__));
+ (*this)(2,1) = cos__q_RH_HAA__;
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(2,3) = ((( 0.35 * cos__q_RH_HFE__) - ( 0.207 * sin__q_RH_HAA__)) + 0.08);
+ (*this)(2,4) = ((( 0.35 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ( 0.3735 * sin__q_RH_HAA__));
+ (*this)(2,5) = (((- 0.35 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - ( 0.3735 * cos__q_RH_HAA__));
+ (*this)(3,3) = ((- cos__q_RH_HFE__ * sin__q_RH_KFE__) - ( sin__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(3,4) = ((( sin__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(3,5) = ((( cos__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__) - (( cos__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(4,3) = (( sin__q_RH_HFE__ * sin__q_RH_KFE__) - ( cos__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(4,4) = (((- sin__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(4,5) = ((( cos__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) + (( cos__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(5,4) = cos__q_RH_HAA__;
+ (*this)(5,5) = sin__q_RH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_LF_hipassemblyCOM::Type_fr_trunk_X_LF_hipassemblyCOM()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = - 1.0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(2,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = - 1.0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_LF_hipassemblyCOM& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_LF_hipassemblyCOM::update(const JState& q) {
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,3) = (- 0.207 * cos__q_LF_HAA__);
+ (*this)(0,4) = (( 0.207 * sin__q_LF_HAA__) - 0.043);
+ (*this)(1,0) = - sin__q_LF_HAA__;
+ (*this)(1,1) = - cos__q_LF_HAA__;
+ (*this)(1,3) = ( 0.2045 * cos__q_LF_HAA__);
+ (*this)(1,4) = (- 0.2045 * sin__q_LF_HAA__);
+ (*this)(1,5) = ( 0.043 * cos__q_LF_HAA__);
+ (*this)(2,0) = - cos__q_LF_HAA__;
+ (*this)(2,1) = sin__q_LF_HAA__;
+ (*this)(2,3) = (- 0.2045 * sin__q_LF_HAA__);
+ (*this)(2,4) = (- 0.2045 * cos__q_LF_HAA__);
+ (*this)(2,5) = ( 0.207 - ( 0.043 * sin__q_LF_HAA__));
+ (*this)(4,3) = - sin__q_LF_HAA__;
+ (*this)(4,4) = - cos__q_LF_HAA__;
+ (*this)(5,3) = - cos__q_LF_HAA__;
+ (*this)(5,4) = sin__q_LF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_RF_hipassemblyCOM::Type_fr_trunk_X_RF_hipassemblyCOM()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 1.0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(2,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 1.0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_RF_hipassemblyCOM& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_RF_hipassemblyCOM::update(const JState& q) {
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,3) = ( 0.207 * cos__q_RF_HAA__);
+ (*this)(0,4) = ( 0.043 - ( 0.207 * sin__q_RF_HAA__));
+ (*this)(1,0) = sin__q_RF_HAA__;
+ (*this)(1,1) = cos__q_RF_HAA__;
+ (*this)(1,3) = ( 0.2045 * cos__q_RF_HAA__);
+ (*this)(1,4) = (- 0.2045 * sin__q_RF_HAA__);
+ (*this)(1,5) = (- 0.043 * cos__q_RF_HAA__);
+ (*this)(2,0) = - cos__q_RF_HAA__;
+ (*this)(2,1) = sin__q_RF_HAA__;
+ (*this)(2,3) = ( 0.2045 * sin__q_RF_HAA__);
+ (*this)(2,4) = ( 0.2045 * cos__q_RF_HAA__);
+ (*this)(2,5) = ( 0.207 - ( 0.043 * sin__q_RF_HAA__));
+ (*this)(4,3) = sin__q_RF_HAA__;
+ (*this)(4,4) = cos__q_RF_HAA__;
+ (*this)(5,3) = - cos__q_RF_HAA__;
+ (*this)(5,4) = sin__q_RF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_LH_hipassemblyCOM::Type_fr_trunk_X_LH_hipassemblyCOM()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = - 1.0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(2,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = - 1.0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_LH_hipassemblyCOM& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_LH_hipassemblyCOM::update(const JState& q) {
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,3) = (- 0.207 * cos__q_LH_HAA__);
+ (*this)(0,4) = (( 0.207 * sin__q_LH_HAA__) - 0.043);
+ (*this)(1,0) = - sin__q_LH_HAA__;
+ (*this)(1,1) = - cos__q_LH_HAA__;
+ (*this)(1,3) = (- 0.2045 * cos__q_LH_HAA__);
+ (*this)(1,4) = ( 0.2045 * sin__q_LH_HAA__);
+ (*this)(1,5) = ( 0.043 * cos__q_LH_HAA__);
+ (*this)(2,0) = - cos__q_LH_HAA__;
+ (*this)(2,1) = sin__q_LH_HAA__;
+ (*this)(2,3) = ( 0.2045 * sin__q_LH_HAA__);
+ (*this)(2,4) = ( 0.2045 * cos__q_LH_HAA__);
+ (*this)(2,5) = ( 0.207 - ( 0.043 * sin__q_LH_HAA__));
+ (*this)(4,3) = - sin__q_LH_HAA__;
+ (*this)(4,4) = - cos__q_LH_HAA__;
+ (*this)(5,3) = - cos__q_LH_HAA__;
+ (*this)(5,4) = sin__q_LH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_RH_hipassemblyCOM::Type_fr_trunk_X_RH_hipassemblyCOM()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 1.0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(2,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 1.0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_RH_hipassemblyCOM& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_RH_hipassemblyCOM::update(const JState& q) {
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,3) = ( 0.207 * cos__q_RH_HAA__);
+ (*this)(0,4) = ( 0.043 - ( 0.207 * sin__q_RH_HAA__));
+ (*this)(1,0) = sin__q_RH_HAA__;
+ (*this)(1,1) = cos__q_RH_HAA__;
+ (*this)(1,3) = (- 0.2045 * cos__q_RH_HAA__);
+ (*this)(1,4) = ( 0.2045 * sin__q_RH_HAA__);
+ (*this)(1,5) = (- 0.043 * cos__q_RH_HAA__);
+ (*this)(2,0) = - cos__q_RH_HAA__;
+ (*this)(2,1) = sin__q_RH_HAA__;
+ (*this)(2,3) = (- 0.2045 * sin__q_RH_HAA__);
+ (*this)(2,4) = (- 0.2045 * cos__q_RH_HAA__);
+ (*this)(2,5) = ( 0.207 - ( 0.043 * sin__q_RH_HAA__));
+ (*this)(4,3) = sin__q_RH_HAA__;
+ (*this)(4,4) = cos__q_RH_HAA__;
+ (*this)(5,3) = - cos__q_RH_HAA__;
+ (*this)(5,4) = sin__q_RH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_LF_upperlegCOM::Type_fr_trunk_X_LF_upperlegCOM()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_LF_upperlegCOM& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_LF_upperlegCOM::update(const JState& q) {
+ Scalar sin__q_LF_HFE__;
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,0) = - sin__q_LF_HFE__;
+ (*this)(0,1) = - cos__q_LF_HFE__;
+ (*this)(0,3) = ((- 0.207 * cos__q_LF_HAA__) * cos__q_LF_HFE__);
+ (*this)(0,4) = (( 0.207 * cos__q_LF_HAA__) * sin__q_LF_HFE__);
+ (*this)(0,5) = (((( 0.026 * sin__q_LF_HFE__) + ( 0.151 * cos__q_LF_HFE__)) - ( 0.207 * sin__q_LF_HAA__)) + 0.08);
+ (*this)(1,0) = (- sin__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(1,1) = ( sin__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(1,2) = cos__q_LF_HAA__;
+ (*this)(1,3) = (((( 0.08 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.3735 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.026 * cos__q_LF_HAA__));
+ (*this)(1,4) = ((((- 0.3735 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.08 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.151 * cos__q_LF_HAA__));
+ (*this)(1,5) = ((((- 0.151 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.026 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.3735 * sin__q_LF_HAA__));
+ (*this)(2,0) = (- cos__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(2,1) = ( cos__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(2,3) = (((( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * sin__q_LF_HFE__) - (( 0.3735 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) - ( 0.026 * sin__q_LF_HAA__));
+ (*this)(2,4) = (((( 0.3735 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * cos__q_LF_HFE__)) - ( 0.151 * sin__q_LF_HAA__));
+ (*this)(2,5) = ((((- 0.151 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.026 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.3735 * cos__q_LF_HAA__));
+ (*this)(3,3) = - sin__q_LF_HFE__;
+ (*this)(3,4) = - cos__q_LF_HFE__;
+ (*this)(4,3) = (- sin__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(4,4) = ( sin__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(4,5) = cos__q_LF_HAA__;
+ (*this)(5,3) = (- cos__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(5,4) = ( cos__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(5,5) = - sin__q_LF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_RF_upperlegCOM::Type_fr_trunk_X_RF_upperlegCOM()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_RF_upperlegCOM& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_RF_upperlegCOM::update(const JState& q) {
+ Scalar sin__q_RF_HFE__;
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,0) = - sin__q_RF_HFE__;
+ (*this)(0,1) = - cos__q_RF_HFE__;
+ (*this)(0,3) = (( 0.207 * cos__q_RF_HAA__) * cos__q_RF_HFE__);
+ (*this)(0,4) = ((- 0.207 * cos__q_RF_HAA__) * sin__q_RF_HFE__);
+ (*this)(0,5) = (((( 0.026 * sin__q_RF_HFE__) + ( 0.151 * cos__q_RF_HFE__)) - ( 0.207 * sin__q_RF_HAA__)) + 0.08);
+ (*this)(1,0) = ( sin__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(1,1) = (- sin__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(1,2) = cos__q_RF_HAA__;
+ (*this)(1,3) = (((( 0.08 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.3735 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.026 * cos__q_RF_HAA__));
+ (*this)(1,4) = ((((- 0.3735 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.08 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.151 * cos__q_RF_HAA__));
+ (*this)(1,5) = (((( 0.151 * sin__q_RF_HAA__) * sin__q_RF_HFE__) - (( 0.026 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) - ( 0.3735 * sin__q_RF_HAA__));
+ (*this)(2,0) = (- cos__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(2,1) = ( cos__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(2,3) = ((((( 0.08 * sin__q_RF_HAA__) - 0.207) * sin__q_RF_HFE__) + (( 0.3735 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.026 * sin__q_RF_HAA__));
+ (*this)(2,4) = ((((- 0.3735 * sin__q_RF_HAA__) * sin__q_RF_HFE__) + ((( 0.08 * sin__q_RF_HAA__) - 0.207) * cos__q_RF_HFE__)) + ( 0.151 * sin__q_RF_HAA__));
+ (*this)(2,5) = ((((- 0.151 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.026 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.3735 * cos__q_RF_HAA__));
+ (*this)(3,3) = - sin__q_RF_HFE__;
+ (*this)(3,4) = - cos__q_RF_HFE__;
+ (*this)(4,3) = ( sin__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(4,4) = (- sin__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(4,5) = cos__q_RF_HAA__;
+ (*this)(5,3) = (- cos__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(5,4) = ( cos__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(5,5) = sin__q_RF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_LH_upperlegCOM::Type_fr_trunk_X_LH_upperlegCOM()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_LH_upperlegCOM& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_LH_upperlegCOM::update(const JState& q) {
+ Scalar sin__q_LH_HFE__;
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,0) = - sin__q_LH_HFE__;
+ (*this)(0,1) = - cos__q_LH_HFE__;
+ (*this)(0,3) = ((- 0.207 * cos__q_LH_HAA__) * cos__q_LH_HFE__);
+ (*this)(0,4) = (( 0.207 * cos__q_LH_HAA__) * sin__q_LH_HFE__);
+ (*this)(0,5) = ((((- 0.026 * sin__q_LH_HFE__) + ( 0.151 * cos__q_LH_HFE__)) - ( 0.207 * sin__q_LH_HAA__)) + 0.08);
+ (*this)(1,0) = (- sin__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(1,1) = ( sin__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(1,2) = cos__q_LH_HAA__;
+ (*this)(1,3) = (((( 0.08 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.3735 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) - ( 0.026 * cos__q_LH_HAA__));
+ (*this)(1,4) = (((( 0.3735 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.08 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) + ( 0.151 * cos__q_LH_HAA__));
+ (*this)(1,5) = ((((- 0.151 * sin__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.026 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) - ( 0.3735 * sin__q_LH_HAA__));
+ (*this)(2,0) = (- cos__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(2,1) = ( cos__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(2,3) = (((( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * sin__q_LH_HFE__) + (( 0.3735 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) + ( 0.026 * sin__q_LH_HAA__));
+ (*this)(2,4) = ((((- 0.3735 * sin__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * cos__q_LH_HFE__)) - ( 0.151 * sin__q_LH_HAA__));
+ (*this)(2,5) = ((((- 0.151 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.026 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) - ( 0.3735 * cos__q_LH_HAA__));
+ (*this)(3,3) = - sin__q_LH_HFE__;
+ (*this)(3,4) = - cos__q_LH_HFE__;
+ (*this)(4,3) = (- sin__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(4,4) = ( sin__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(4,5) = cos__q_LH_HAA__;
+ (*this)(5,3) = (- cos__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(5,4) = ( cos__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(5,5) = - sin__q_LH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_RH_upperlegCOM::Type_fr_trunk_X_RH_upperlegCOM()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_RH_upperlegCOM& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_RH_upperlegCOM::update(const JState& q) {
+ Scalar sin__q_RH_HFE__;
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,0) = - sin__q_RH_HFE__;
+ (*this)(0,1) = - cos__q_RH_HFE__;
+ (*this)(0,3) = (( 0.207 * cos__q_RH_HAA__) * cos__q_RH_HFE__);
+ (*this)(0,4) = ((- 0.207 * cos__q_RH_HAA__) * sin__q_RH_HFE__);
+ (*this)(0,5) = ((((- 0.026 * sin__q_RH_HFE__) + ( 0.151 * cos__q_RH_HFE__)) - ( 0.207 * sin__q_RH_HAA__)) + 0.08);
+ (*this)(1,0) = ( sin__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(1,1) = (- sin__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(1,2) = cos__q_RH_HAA__;
+ (*this)(1,3) = (((( 0.08 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.3735 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) - ( 0.026 * cos__q_RH_HAA__));
+ (*this)(1,4) = (((( 0.3735 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.08 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.151 * cos__q_RH_HAA__));
+ (*this)(1,5) = (((( 0.151 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.026 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.3735 * sin__q_RH_HAA__));
+ (*this)(2,0) = (- cos__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(2,1) = ( cos__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(2,3) = ((((( 0.08 * sin__q_RH_HAA__) - 0.207) * sin__q_RH_HFE__) - (( 0.3735 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) - ( 0.026 * sin__q_RH_HAA__));
+ (*this)(2,4) = (((( 0.3735 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ((( 0.08 * sin__q_RH_HAA__) - 0.207) * cos__q_RH_HFE__)) + ( 0.151 * sin__q_RH_HAA__));
+ (*this)(2,5) = ((((- 0.151 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.026 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) - ( 0.3735 * cos__q_RH_HAA__));
+ (*this)(3,3) = - sin__q_RH_HFE__;
+ (*this)(3,4) = - cos__q_RH_HFE__;
+ (*this)(4,3) = ( sin__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(4,4) = (- sin__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(4,5) = cos__q_RH_HAA__;
+ (*this)(5,3) = (- cos__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(5,4) = ( cos__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(5,5) = sin__q_RH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_LF_lowerlegCOM::Type_fr_trunk_X_LF_lowerlegCOM()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_LF_lowerlegCOM& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_LF_lowerlegCOM::update(const JState& q) {
+ Scalar sin__q_LF_KFE__;
+ Scalar sin__q_LF_HFE__;
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+ Scalar cos__q_LF_KFE__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_KFE__ = TRAIT::sin( q(LF_KFE));
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+ cos__q_LF_KFE__ = TRAIT::cos( q(LF_KFE));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,0) = ((- cos__q_LF_HFE__ * sin__q_LF_KFE__) - ( sin__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(0,1) = (( sin__q_LF_HFE__ * sin__q_LF_KFE__) - ( cos__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(0,3) = (((( 0.207 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * sin__q_LF_KFE__) - ((( 0.207 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(0,4) = (((( 0.207 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) + ((( 0.207 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(0,5) = ((((((- 0.125 * sin__q_LF_HFE__) * sin__q_LF_KFE__) + (( 0.125 * cos__q_LF_HFE__) * cos__q_LF_KFE__)) + ( 0.35 * cos__q_LF_HFE__)) - ( 0.207 * sin__q_LF_HAA__)) + 0.08);
+ (*this)(1,0) = ((( sin__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( sin__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,1) = ((( sin__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( sin__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,2) = cos__q_LF_HAA__;
+ (*this)(1,3) = ((((((- 0.3735 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.08 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.35 * cos__q_LF_HAA__)) * sin__q_LF_KFE__) + (((( 0.08 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.3735 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) * cos__q_LF_KFE__));
+ (*this)(1,4) = ((((((- 0.08 * cos__q_LF_HAA__) * sin__q_LF_HFE__) - (( 0.3735 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) * sin__q_LF_KFE__) + (((((- 0.3735 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.08 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.35 * cos__q_LF_HAA__)) * cos__q_LF_KFE__)) + ( 0.125 * cos__q_LF_HAA__));
+ (*this)(1,5) = ((((((- 0.125 * sin__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) - ((( 0.125 * sin__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__)) - (( 0.35 * sin__q_LF_HAA__) * sin__q_LF_HFE__)) + ( 0.3735 * sin__q_LF_HAA__));
+ (*this)(2,0) = ((( cos__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( cos__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(2,1) = ((( cos__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( cos__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(2,3) = (((((( 0.3735 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * cos__q_LF_HFE__)) - ( 0.35 * sin__q_LF_HAA__)) * sin__q_LF_KFE__) + (((( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * sin__q_LF_HFE__) - (( 0.3735 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) * cos__q_LF_KFE__));
+ (*this)(2,4) = ((((((( 0.08 * sin__q_LF_HAA__) - 0.207) * sin__q_LF_HFE__) + (( 0.3735 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) * sin__q_LF_KFE__) + ((((( 0.3735 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * cos__q_LF_HFE__)) - ( 0.35 * sin__q_LF_HAA__)) * cos__q_LF_KFE__)) - ( 0.125 * sin__q_LF_HAA__));
+ (*this)(2,5) = ((((((- 0.125 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) - ((( 0.125 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__)) - (( 0.35 * cos__q_LF_HAA__) * sin__q_LF_HFE__)) + ( 0.3735 * cos__q_LF_HAA__));
+ (*this)(3,3) = ((- cos__q_LF_HFE__ * sin__q_LF_KFE__) - ( sin__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(3,4) = (( sin__q_LF_HFE__ * sin__q_LF_KFE__) - ( cos__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(4,3) = ((( sin__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( sin__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(4,4) = ((( sin__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( sin__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(4,5) = cos__q_LF_HAA__;
+ (*this)(5,3) = ((( cos__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( cos__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(5,4) = ((( cos__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( cos__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(5,5) = - sin__q_LF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_RF_lowerlegCOM::Type_fr_trunk_X_RF_lowerlegCOM()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_RF_lowerlegCOM& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_RF_lowerlegCOM::update(const JState& q) {
+ Scalar sin__q_RF_KFE__;
+ Scalar sin__q_RF_HFE__;
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+ Scalar cos__q_RF_KFE__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_KFE__ = TRAIT::sin( q(RF_KFE));
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+ cos__q_RF_KFE__ = TRAIT::cos( q(RF_KFE));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,0) = ((- cos__q_RF_HFE__ * sin__q_RF_KFE__) - ( sin__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(0,1) = (( sin__q_RF_HFE__ * sin__q_RF_KFE__) - ( cos__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(0,3) = (((( 0.207 * cos__q_RF_HAA__) * cos__q_RF_HFE__) * cos__q_RF_KFE__) - ((( 0.207 * cos__q_RF_HAA__) * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(0,4) = ((((- 0.207 * cos__q_RF_HAA__) * cos__q_RF_HFE__) * sin__q_RF_KFE__) - ((( 0.207 * cos__q_RF_HAA__) * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(0,5) = (((((((- 0.125 * sin__q_RF_HFE__) - ( 0.001 * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + ((( 0.125 * cos__q_RF_HFE__) - ( 0.001 * sin__q_RF_HFE__)) * cos__q_RF_KFE__)) + ( 0.35 * cos__q_RF_HFE__)) - ( 0.207 * sin__q_RF_HAA__)) + 0.08);
+ (*this)(1,0) = ((( sin__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(1,1) = (((- sin__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(1,2) = cos__q_RF_HAA__;
+ (*this)(1,3) = (((((((- 0.3735 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.08 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.35 * cos__q_RF_HAA__)) * sin__q_RF_KFE__) + (((( 0.08 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.3735 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) * cos__q_RF_KFE__)) - ( 0.001 * cos__q_RF_HAA__));
+ (*this)(1,4) = ((((((- 0.08 * cos__q_RF_HAA__) * sin__q_RF_HFE__) - (( 0.3735 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + (((((- 0.3735 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.08 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.35 * cos__q_RF_HAA__)) * cos__q_RF_KFE__)) + ( 0.125 * cos__q_RF_HAA__));
+ (*this)(1,5) = ((((((( 0.125 * sin__q_RF_HAA__) * cos__q_RF_HFE__) - (( 0.001 * sin__q_RF_HAA__) * sin__q_RF_HFE__)) * sin__q_RF_KFE__) + (((( 0.125 * sin__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.001 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) * cos__q_RF_KFE__)) + (( 0.35 * sin__q_RF_HAA__) * sin__q_RF_HFE__)) - ( 0.3735 * sin__q_RF_HAA__));
+ (*this)(2,0) = ((( cos__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__) - (( cos__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(2,1) = ((( cos__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) + (( cos__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(2,3) = (((((((- 0.3735 * sin__q_RF_HAA__) * sin__q_RF_HFE__) + ((( 0.08 * sin__q_RF_HAA__) - 0.207) * cos__q_RF_HFE__)) + ( 0.35 * sin__q_RF_HAA__)) * sin__q_RF_KFE__) + ((((( 0.08 * sin__q_RF_HAA__) - 0.207) * sin__q_RF_HFE__) + (( 0.3735 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) * cos__q_RF_KFE__)) - ( 0.001 * sin__q_RF_HAA__));
+ (*this)(2,4) = (((((( 0.207 - ( 0.08 * sin__q_RF_HAA__)) * sin__q_RF_HFE__) - (( 0.3735 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + (((((- 0.3735 * sin__q_RF_HAA__) * sin__q_RF_HFE__) + ((( 0.08 * sin__q_RF_HAA__) - 0.207) * cos__q_RF_HFE__)) + ( 0.35 * sin__q_RF_HAA__)) * cos__q_RF_KFE__)) + ( 0.125 * sin__q_RF_HAA__));
+ (*this)(2,5) = ((((((( 0.001 * cos__q_RF_HAA__) * sin__q_RF_HFE__) - (( 0.125 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + ((((- 0.125 * cos__q_RF_HAA__) * sin__q_RF_HFE__) - (( 0.001 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) * cos__q_RF_KFE__)) - (( 0.35 * cos__q_RF_HAA__) * sin__q_RF_HFE__)) + ( 0.3735 * cos__q_RF_HAA__));
+ (*this)(3,3) = ((- cos__q_RF_HFE__ * sin__q_RF_KFE__) - ( sin__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(3,4) = (( sin__q_RF_HFE__ * sin__q_RF_KFE__) - ( cos__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(4,3) = ((( sin__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(4,4) = (((- sin__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(4,5) = cos__q_RF_HAA__;
+ (*this)(5,3) = ((( cos__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__) - (( cos__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(5,4) = ((( cos__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) + (( cos__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(5,5) = sin__q_RF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_LH_lowerlegCOM::Type_fr_trunk_X_LH_lowerlegCOM()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_LH_lowerlegCOM& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_LH_lowerlegCOM::update(const JState& q) {
+ Scalar sin__q_LH_KFE__;
+ Scalar sin__q_LH_HFE__;
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+ Scalar cos__q_LH_KFE__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_KFE__ = TRAIT::sin( q(LH_KFE));
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+ cos__q_LH_KFE__ = TRAIT::cos( q(LH_KFE));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,0) = ((- cos__q_LH_HFE__ * sin__q_LH_KFE__) - ( sin__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(0,1) = (( sin__q_LH_HFE__ * sin__q_LH_KFE__) - ( cos__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(0,3) = (((( 0.207 * cos__q_LH_HAA__) * sin__q_LH_HFE__) * sin__q_LH_KFE__) - ((( 0.207 * cos__q_LH_HAA__) * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(0,4) = (((( 0.207 * cos__q_LH_HAA__) * cos__q_LH_HFE__) * sin__q_LH_KFE__) + ((( 0.207 * cos__q_LH_HAA__) * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(0,5) = ((((((( 0.001 * cos__q_LH_HFE__) - ( 0.125 * sin__q_LH_HFE__)) * sin__q_LH_KFE__) + ((( 0.001 * sin__q_LH_HFE__) + ( 0.125 * cos__q_LH_HFE__)) * cos__q_LH_KFE__)) + ( 0.35 * cos__q_LH_HFE__)) - ( 0.207 * sin__q_LH_HAA__)) + 0.08);
+ (*this)(1,0) = ((( sin__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( sin__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,1) = ((( sin__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( sin__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,2) = cos__q_LH_HAA__;
+ (*this)(1,3) = ((((((( 0.3735 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.08 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) + ( 0.35 * cos__q_LH_HAA__)) * sin__q_LH_KFE__) + (((( 0.08 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.3735 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) * cos__q_LH_KFE__)) + ( 0.001 * cos__q_LH_HAA__));
+ (*this)(1,4) = (((((( 0.3735 * cos__q_LH_HAA__) * cos__q_LH_HFE__) - (( 0.08 * cos__q_LH_HAA__) * sin__q_LH_HFE__)) * sin__q_LH_KFE__) + ((((( 0.3735 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.08 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) + ( 0.35 * cos__q_LH_HAA__)) * cos__q_LH_KFE__)) + ( 0.125 * cos__q_LH_HAA__));
+ (*this)(1,5) = (((((((- 0.001 * sin__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.125 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) * sin__q_LH_KFE__) + (((( 0.001 * sin__q_LH_HAA__) * cos__q_LH_HFE__) - (( 0.125 * sin__q_LH_HAA__) * sin__q_LH_HFE__)) * cos__q_LH_KFE__)) - (( 0.35 * sin__q_LH_HAA__) * sin__q_LH_HFE__)) - ( 0.3735 * sin__q_LH_HAA__));
+ (*this)(2,0) = ((( cos__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( cos__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(2,1) = ((( cos__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( cos__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(2,3) = (((((((- 0.3735 * sin__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * cos__q_LH_HFE__)) - ( 0.35 * sin__q_LH_HAA__)) * sin__q_LH_KFE__) + (((( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * sin__q_LH_HFE__) + (( 0.3735 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) * cos__q_LH_KFE__)) - ( 0.001 * sin__q_LH_HAA__));
+ (*this)(2,4) = ((((((( 0.08 * sin__q_LH_HAA__) - 0.207) * sin__q_LH_HFE__) - (( 0.3735 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) * sin__q_LH_KFE__) + (((((- 0.3735 * sin__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * cos__q_LH_HFE__)) - ( 0.35 * sin__q_LH_HAA__)) * cos__q_LH_KFE__)) - ( 0.125 * sin__q_LH_HAA__));
+ (*this)(2,5) = (((((((- 0.001 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.125 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) * sin__q_LH_KFE__) + (((( 0.001 * cos__q_LH_HAA__) * cos__q_LH_HFE__) - (( 0.125 * cos__q_LH_HAA__) * sin__q_LH_HFE__)) * cos__q_LH_KFE__)) - (( 0.35 * cos__q_LH_HAA__) * sin__q_LH_HFE__)) - ( 0.3735 * cos__q_LH_HAA__));
+ (*this)(3,3) = ((- cos__q_LH_HFE__ * sin__q_LH_KFE__) - ( sin__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(3,4) = (( sin__q_LH_HFE__ * sin__q_LH_KFE__) - ( cos__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(4,3) = ((( sin__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( sin__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(4,4) = ((( sin__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( sin__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(4,5) = cos__q_LH_HAA__;
+ (*this)(5,3) = ((( cos__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( cos__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(5,4) = ((( cos__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( cos__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(5,5) = - sin__q_LH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_RH_lowerlegCOM::Type_fr_trunk_X_RH_lowerlegCOM()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_RH_lowerlegCOM& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_RH_lowerlegCOM::update(const JState& q) {
+ Scalar sin__q_RH_KFE__;
+ Scalar sin__q_RH_HFE__;
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+ Scalar cos__q_RH_KFE__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_KFE__ = TRAIT::sin( q(RH_KFE));
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+ cos__q_RH_KFE__ = TRAIT::cos( q(RH_KFE));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,0) = ((- cos__q_RH_HFE__ * sin__q_RH_KFE__) - ( sin__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(0,1) = (( sin__q_RH_HFE__ * sin__q_RH_KFE__) - ( cos__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(0,3) = (((( 0.207 * cos__q_RH_HAA__) * cos__q_RH_HFE__) * cos__q_RH_KFE__) - ((( 0.207 * cos__q_RH_HAA__) * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(0,4) = ((((- 0.207 * cos__q_RH_HAA__) * cos__q_RH_HFE__) * sin__q_RH_KFE__) - ((( 0.207 * cos__q_RH_HAA__) * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(0,5) = ((((((( 0.001 * cos__q_RH_HFE__) - ( 0.125 * sin__q_RH_HFE__)) * sin__q_RH_KFE__) + ((( 0.001 * sin__q_RH_HFE__) + ( 0.125 * cos__q_RH_HFE__)) * cos__q_RH_KFE__)) + ( 0.35 * cos__q_RH_HFE__)) - ( 0.207 * sin__q_RH_HAA__)) + 0.08);
+ (*this)(1,0) = ((( sin__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(1,1) = (((- sin__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(1,2) = cos__q_RH_HAA__;
+ (*this)(1,3) = ((((((( 0.3735 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.08 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.35 * cos__q_RH_HAA__)) * sin__q_RH_KFE__) + (((( 0.08 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.3735 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) * cos__q_RH_KFE__)) + ( 0.001 * cos__q_RH_HAA__));
+ (*this)(1,4) = (((((( 0.3735 * cos__q_RH_HAA__) * cos__q_RH_HFE__) - (( 0.08 * cos__q_RH_HAA__) * sin__q_RH_HFE__)) * sin__q_RH_KFE__) + ((((( 0.3735 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.08 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.35 * cos__q_RH_HAA__)) * cos__q_RH_KFE__)) + ( 0.125 * cos__q_RH_HAA__));
+ (*this)(1,5) = ((((((( 0.001 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.125 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) * sin__q_RH_KFE__) + (((( 0.125 * sin__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.001 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) * cos__q_RH_KFE__)) + (( 0.35 * sin__q_RH_HAA__) * sin__q_RH_HFE__)) + ( 0.3735 * sin__q_RH_HAA__));
+ (*this)(2,0) = ((( cos__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__) - (( cos__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(2,1) = ((( cos__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) + (( cos__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(2,3) = ((((((( 0.3735 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ((( 0.08 * sin__q_RH_HAA__) - 0.207) * cos__q_RH_HFE__)) + ( 0.35 * sin__q_RH_HAA__)) * sin__q_RH_KFE__) + ((((( 0.08 * sin__q_RH_HAA__) - 0.207) * sin__q_RH_HFE__) - (( 0.3735 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) * cos__q_RH_KFE__)) + ( 0.001 * sin__q_RH_HAA__));
+ (*this)(2,4) = (((((( 0.207 - ( 0.08 * sin__q_RH_HAA__)) * sin__q_RH_HFE__) + (( 0.3735 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) * sin__q_RH_KFE__) + ((((( 0.3735 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ((( 0.08 * sin__q_RH_HAA__) - 0.207) * cos__q_RH_HFE__)) + ( 0.35 * sin__q_RH_HAA__)) * cos__q_RH_KFE__)) + ( 0.125 * sin__q_RH_HAA__));
+ (*this)(2,5) = (((((((- 0.001 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.125 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) * sin__q_RH_KFE__) + (((( 0.001 * cos__q_RH_HAA__) * cos__q_RH_HFE__) - (( 0.125 * cos__q_RH_HAA__) * sin__q_RH_HFE__)) * cos__q_RH_KFE__)) - (( 0.35 * cos__q_RH_HAA__) * sin__q_RH_HFE__)) - ( 0.3735 * cos__q_RH_HAA__));
+ (*this)(3,3) = ((- cos__q_RH_HFE__ * sin__q_RH_KFE__) - ( sin__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(3,4) = (( sin__q_RH_HFE__ * sin__q_RH_KFE__) - ( cos__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(4,3) = ((( sin__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(4,4) = (((- sin__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(4,5) = cos__q_RH_HAA__;
+ (*this)(5,3) = ((( cos__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__) - (( cos__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(5,4) = ((( cos__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) + (( cos__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(5,5) = sin__q_RH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LF_foot_X_fr_LF_lowerleg::Type_fr_LF_foot_X_fr_LF_lowerleg()
+{
+ (*this)(0,0) = 1;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 1;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0.33;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = - 0.33;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = 1;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LF_foot_X_fr_LF_lowerleg& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LF_foot_X_fr_LF_lowerleg::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RF_foot_X_fr_RF_lowerleg::Type_fr_RF_foot_X_fr_RF_lowerleg()
+{
+ (*this)(0,0) = 1;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 1;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0.33;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = - 0.33;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = 1;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RF_foot_X_fr_RF_lowerleg& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RF_foot_X_fr_RF_lowerleg::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LH_foot_X_fr_LH_lowerleg::Type_fr_LH_foot_X_fr_LH_lowerleg()
+{
+ (*this)(0,0) = 1;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 1;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0.33;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = - 0.33;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = 1;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LH_foot_X_fr_LH_lowerleg& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LH_foot_X_fr_LH_lowerleg::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RH_foot_X_fr_RH_lowerleg::Type_fr_RH_foot_X_fr_RH_lowerleg()
+{
+ (*this)(0,0) = 1;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 1;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0.33;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = - 0.33;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = 1;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RH_foot_X_fr_RH_lowerleg& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RH_foot_X_fr_RH_lowerleg::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_foot::Type_fr_trunk_X_fr_LF_foot()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_foot& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_foot::update(const JState& q) {
+ Scalar sin__q_LF_KFE__;
+ Scalar sin__q_LF_HFE__;
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+ Scalar cos__q_LF_KFE__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_KFE__ = TRAIT::sin( q(LF_KFE));
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+ cos__q_LF_KFE__ = TRAIT::cos( q(LF_KFE));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,0) = ((- cos__q_LF_HFE__ * sin__q_LF_KFE__) - ( sin__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(0,1) = (( sin__q_LF_HFE__ * sin__q_LF_KFE__) - ( cos__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(0,3) = (((( 0.207 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * sin__q_LF_KFE__) - ((( 0.207 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(0,4) = (((( 0.207 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) + ((( 0.207 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(0,5) = ((((((- 0.33 * sin__q_LF_HFE__) * sin__q_LF_KFE__) + (( 0.33 * cos__q_LF_HFE__) * cos__q_LF_KFE__)) + ( 0.35 * cos__q_LF_HFE__)) - ( 0.207 * sin__q_LF_HAA__)) + 0.08);
+ (*this)(1,0) = ((( sin__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( sin__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,1) = ((( sin__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( sin__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,2) = cos__q_LF_HAA__;
+ (*this)(1,3) = ((((((- 0.3735 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.08 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.35 * cos__q_LF_HAA__)) * sin__q_LF_KFE__) + (((( 0.08 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.3735 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) * cos__q_LF_KFE__));
+ (*this)(1,4) = ((((((- 0.08 * cos__q_LF_HAA__) * sin__q_LF_HFE__) - (( 0.3735 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) * sin__q_LF_KFE__) + (((((- 0.3735 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.08 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.35 * cos__q_LF_HAA__)) * cos__q_LF_KFE__)) + ( 0.33 * cos__q_LF_HAA__));
+ (*this)(1,5) = ((((((- 0.33 * sin__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) - ((( 0.33 * sin__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__)) - (( 0.35 * sin__q_LF_HAA__) * sin__q_LF_HFE__)) + ( 0.3735 * sin__q_LF_HAA__));
+ (*this)(2,0) = ((( cos__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( cos__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(2,1) = ((( cos__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( cos__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(2,3) = (((((( 0.3735 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * cos__q_LF_HFE__)) - ( 0.35 * sin__q_LF_HAA__)) * sin__q_LF_KFE__) + (((( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * sin__q_LF_HFE__) - (( 0.3735 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) * cos__q_LF_KFE__));
+ (*this)(2,4) = ((((((( 0.08 * sin__q_LF_HAA__) - 0.207) * sin__q_LF_HFE__) + (( 0.3735 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) * sin__q_LF_KFE__) + ((((( 0.3735 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * cos__q_LF_HFE__)) - ( 0.35 * sin__q_LF_HAA__)) * cos__q_LF_KFE__)) - ( 0.33 * sin__q_LF_HAA__));
+ (*this)(2,5) = ((((((- 0.33 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) - ((( 0.33 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__)) - (( 0.35 * cos__q_LF_HAA__) * sin__q_LF_HFE__)) + ( 0.3735 * cos__q_LF_HAA__));
+ (*this)(3,3) = ((- cos__q_LF_HFE__ * sin__q_LF_KFE__) - ( sin__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(3,4) = (( sin__q_LF_HFE__ * sin__q_LF_KFE__) - ( cos__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(4,3) = ((( sin__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( sin__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(4,4) = ((( sin__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( sin__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(4,5) = cos__q_LF_HAA__;
+ (*this)(5,3) = ((( cos__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( cos__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(5,4) = ((( cos__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( cos__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(5,5) = - sin__q_LF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_foot::Type_fr_trunk_X_fr_RF_foot()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_foot& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_foot::update(const JState& q) {
+ Scalar sin__q_RF_KFE__;
+ Scalar sin__q_RF_HFE__;
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+ Scalar cos__q_RF_KFE__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_KFE__ = TRAIT::sin( q(RF_KFE));
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+ cos__q_RF_KFE__ = TRAIT::cos( q(RF_KFE));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,0) = ((- cos__q_RF_HFE__ * sin__q_RF_KFE__) - ( sin__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(0,1) = (( sin__q_RF_HFE__ * sin__q_RF_KFE__) - ( cos__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(0,3) = (((( 0.207 * cos__q_RF_HAA__) * cos__q_RF_HFE__) * cos__q_RF_KFE__) - ((( 0.207 * cos__q_RF_HAA__) * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(0,4) = ((((- 0.207 * cos__q_RF_HAA__) * cos__q_RF_HFE__) * sin__q_RF_KFE__) - ((( 0.207 * cos__q_RF_HAA__) * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(0,5) = ((((((- 0.33 * sin__q_RF_HFE__) * sin__q_RF_KFE__) + (( 0.33 * cos__q_RF_HFE__) * cos__q_RF_KFE__)) + ( 0.35 * cos__q_RF_HFE__)) - ( 0.207 * sin__q_RF_HAA__)) + 0.08);
+ (*this)(1,0) = ((( sin__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(1,1) = (((- sin__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(1,2) = cos__q_RF_HAA__;
+ (*this)(1,3) = ((((((- 0.3735 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.08 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.35 * cos__q_RF_HAA__)) * sin__q_RF_KFE__) + (((( 0.08 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.3735 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) * cos__q_RF_KFE__));
+ (*this)(1,4) = ((((((- 0.08 * cos__q_RF_HAA__) * sin__q_RF_HFE__) - (( 0.3735 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + (((((- 0.3735 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.08 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.35 * cos__q_RF_HAA__)) * cos__q_RF_KFE__)) + ( 0.33 * cos__q_RF_HAA__));
+ (*this)(1,5) = (((((( 0.33 * sin__q_RF_HAA__) * cos__q_RF_HFE__) * sin__q_RF_KFE__) + ((( 0.33 * sin__q_RF_HAA__) * sin__q_RF_HFE__) * cos__q_RF_KFE__)) + (( 0.35 * sin__q_RF_HAA__) * sin__q_RF_HFE__)) - ( 0.3735 * sin__q_RF_HAA__));
+ (*this)(2,0) = ((( cos__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__) - (( cos__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(2,1) = ((( cos__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) + (( cos__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(2,3) = ((((((- 0.3735 * sin__q_RF_HAA__) * sin__q_RF_HFE__) + ((( 0.08 * sin__q_RF_HAA__) - 0.207) * cos__q_RF_HFE__)) + ( 0.35 * sin__q_RF_HAA__)) * sin__q_RF_KFE__) + ((((( 0.08 * sin__q_RF_HAA__) - 0.207) * sin__q_RF_HFE__) + (( 0.3735 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) * cos__q_RF_KFE__));
+ (*this)(2,4) = (((((( 0.207 - ( 0.08 * sin__q_RF_HAA__)) * sin__q_RF_HFE__) - (( 0.3735 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + (((((- 0.3735 * sin__q_RF_HAA__) * sin__q_RF_HFE__) + ((( 0.08 * sin__q_RF_HAA__) - 0.207) * cos__q_RF_HFE__)) + ( 0.35 * sin__q_RF_HAA__)) * cos__q_RF_KFE__)) + ( 0.33 * sin__q_RF_HAA__));
+ (*this)(2,5) = ((((((- 0.33 * cos__q_RF_HAA__) * cos__q_RF_HFE__) * sin__q_RF_KFE__) - ((( 0.33 * cos__q_RF_HAA__) * sin__q_RF_HFE__) * cos__q_RF_KFE__)) - (( 0.35 * cos__q_RF_HAA__) * sin__q_RF_HFE__)) + ( 0.3735 * cos__q_RF_HAA__));
+ (*this)(3,3) = ((- cos__q_RF_HFE__ * sin__q_RF_KFE__) - ( sin__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(3,4) = (( sin__q_RF_HFE__ * sin__q_RF_KFE__) - ( cos__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(4,3) = ((( sin__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(4,4) = (((- sin__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(4,5) = cos__q_RF_HAA__;
+ (*this)(5,3) = ((( cos__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__) - (( cos__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(5,4) = ((( cos__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) + (( cos__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(5,5) = sin__q_RF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_foot::Type_fr_trunk_X_fr_LH_foot()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_foot& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_foot::update(const JState& q) {
+ Scalar sin__q_LH_KFE__;
+ Scalar sin__q_LH_HFE__;
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+ Scalar cos__q_LH_KFE__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_KFE__ = TRAIT::sin( q(LH_KFE));
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+ cos__q_LH_KFE__ = TRAIT::cos( q(LH_KFE));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,0) = ((- cos__q_LH_HFE__ * sin__q_LH_KFE__) - ( sin__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(0,1) = (( sin__q_LH_HFE__ * sin__q_LH_KFE__) - ( cos__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(0,3) = (((( 0.207 * cos__q_LH_HAA__) * sin__q_LH_HFE__) * sin__q_LH_KFE__) - ((( 0.207 * cos__q_LH_HAA__) * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(0,4) = (((( 0.207 * cos__q_LH_HAA__) * cos__q_LH_HFE__) * sin__q_LH_KFE__) + ((( 0.207 * cos__q_LH_HAA__) * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(0,5) = ((((((- 0.33 * sin__q_LH_HFE__) * sin__q_LH_KFE__) + (( 0.33 * cos__q_LH_HFE__) * cos__q_LH_KFE__)) + ( 0.35 * cos__q_LH_HFE__)) - ( 0.207 * sin__q_LH_HAA__)) + 0.08);
+ (*this)(1,0) = ((( sin__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( sin__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,1) = ((( sin__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( sin__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,2) = cos__q_LH_HAA__;
+ (*this)(1,3) = (((((( 0.3735 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.08 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) + ( 0.35 * cos__q_LH_HAA__)) * sin__q_LH_KFE__) + (((( 0.08 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.3735 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) * cos__q_LH_KFE__));
+ (*this)(1,4) = (((((( 0.3735 * cos__q_LH_HAA__) * cos__q_LH_HFE__) - (( 0.08 * cos__q_LH_HAA__) * sin__q_LH_HFE__)) * sin__q_LH_KFE__) + ((((( 0.3735 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.08 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) + ( 0.35 * cos__q_LH_HAA__)) * cos__q_LH_KFE__)) + ( 0.33 * cos__q_LH_HAA__));
+ (*this)(1,5) = ((((((- 0.33 * sin__q_LH_HAA__) * cos__q_LH_HFE__) * sin__q_LH_KFE__) - ((( 0.33 * sin__q_LH_HAA__) * sin__q_LH_HFE__) * cos__q_LH_KFE__)) - (( 0.35 * sin__q_LH_HAA__) * sin__q_LH_HFE__)) - ( 0.3735 * sin__q_LH_HAA__));
+ (*this)(2,0) = ((( cos__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( cos__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(2,1) = ((( cos__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( cos__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(2,3) = ((((((- 0.3735 * sin__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * cos__q_LH_HFE__)) - ( 0.35 * sin__q_LH_HAA__)) * sin__q_LH_KFE__) + (((( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * sin__q_LH_HFE__) + (( 0.3735 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) * cos__q_LH_KFE__));
+ (*this)(2,4) = ((((((( 0.08 * sin__q_LH_HAA__) - 0.207) * sin__q_LH_HFE__) - (( 0.3735 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) * sin__q_LH_KFE__) + (((((- 0.3735 * sin__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * cos__q_LH_HFE__)) - ( 0.35 * sin__q_LH_HAA__)) * cos__q_LH_KFE__)) - ( 0.33 * sin__q_LH_HAA__));
+ (*this)(2,5) = ((((((- 0.33 * cos__q_LH_HAA__) * cos__q_LH_HFE__) * sin__q_LH_KFE__) - ((( 0.33 * cos__q_LH_HAA__) * sin__q_LH_HFE__) * cos__q_LH_KFE__)) - (( 0.35 * cos__q_LH_HAA__) * sin__q_LH_HFE__)) - ( 0.3735 * cos__q_LH_HAA__));
+ (*this)(3,3) = ((- cos__q_LH_HFE__ * sin__q_LH_KFE__) - ( sin__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(3,4) = (( sin__q_LH_HFE__ * sin__q_LH_KFE__) - ( cos__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(4,3) = ((( sin__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( sin__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(4,4) = ((( sin__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( sin__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(4,5) = cos__q_LH_HAA__;
+ (*this)(5,3) = ((( cos__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( cos__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(5,4) = ((( cos__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( cos__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(5,5) = - sin__q_LH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_foot::Type_fr_trunk_X_fr_RH_foot()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_foot& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_foot::update(const JState& q) {
+ Scalar sin__q_RH_KFE__;
+ Scalar sin__q_RH_HFE__;
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+ Scalar cos__q_RH_KFE__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_KFE__ = TRAIT::sin( q(RH_KFE));
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+ cos__q_RH_KFE__ = TRAIT::cos( q(RH_KFE));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,0) = ((- cos__q_RH_HFE__ * sin__q_RH_KFE__) - ( sin__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(0,1) = (( sin__q_RH_HFE__ * sin__q_RH_KFE__) - ( cos__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(0,3) = (((( 0.207 * cos__q_RH_HAA__) * cos__q_RH_HFE__) * cos__q_RH_KFE__) - ((( 0.207 * cos__q_RH_HAA__) * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(0,4) = ((((- 0.207 * cos__q_RH_HAA__) * cos__q_RH_HFE__) * sin__q_RH_KFE__) - ((( 0.207 * cos__q_RH_HAA__) * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(0,5) = ((((((- 0.33 * sin__q_RH_HFE__) * sin__q_RH_KFE__) + (( 0.33 * cos__q_RH_HFE__) * cos__q_RH_KFE__)) + ( 0.35 * cos__q_RH_HFE__)) - ( 0.207 * sin__q_RH_HAA__)) + 0.08);
+ (*this)(1,0) = ((( sin__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(1,1) = (((- sin__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(1,2) = cos__q_RH_HAA__;
+ (*this)(1,3) = (((((( 0.3735 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.08 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.35 * cos__q_RH_HAA__)) * sin__q_RH_KFE__) + (((( 0.08 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.3735 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) * cos__q_RH_KFE__));
+ (*this)(1,4) = (((((( 0.3735 * cos__q_RH_HAA__) * cos__q_RH_HFE__) - (( 0.08 * cos__q_RH_HAA__) * sin__q_RH_HFE__)) * sin__q_RH_KFE__) + ((((( 0.3735 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.08 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.35 * cos__q_RH_HAA__)) * cos__q_RH_KFE__)) + ( 0.33 * cos__q_RH_HAA__));
+ (*this)(1,5) = (((((( 0.33 * sin__q_RH_HAA__) * cos__q_RH_HFE__) * sin__q_RH_KFE__) + ((( 0.33 * sin__q_RH_HAA__) * sin__q_RH_HFE__) * cos__q_RH_KFE__)) + (( 0.35 * sin__q_RH_HAA__) * sin__q_RH_HFE__)) + ( 0.3735 * sin__q_RH_HAA__));
+ (*this)(2,0) = ((( cos__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__) - (( cos__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(2,1) = ((( cos__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) + (( cos__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(2,3) = (((((( 0.3735 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ((( 0.08 * sin__q_RH_HAA__) - 0.207) * cos__q_RH_HFE__)) + ( 0.35 * sin__q_RH_HAA__)) * sin__q_RH_KFE__) + ((((( 0.08 * sin__q_RH_HAA__) - 0.207) * sin__q_RH_HFE__) - (( 0.3735 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) * cos__q_RH_KFE__));
+ (*this)(2,4) = (((((( 0.207 - ( 0.08 * sin__q_RH_HAA__)) * sin__q_RH_HFE__) + (( 0.3735 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) * sin__q_RH_KFE__) + ((((( 0.3735 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ((( 0.08 * sin__q_RH_HAA__) - 0.207) * cos__q_RH_HFE__)) + ( 0.35 * sin__q_RH_HAA__)) * cos__q_RH_KFE__)) + ( 0.33 * sin__q_RH_HAA__));
+ (*this)(2,5) = ((((((- 0.33 * cos__q_RH_HAA__) * cos__q_RH_HFE__) * sin__q_RH_KFE__) - ((( 0.33 * cos__q_RH_HAA__) * sin__q_RH_HFE__) * cos__q_RH_KFE__)) - (( 0.35 * cos__q_RH_HAA__) * sin__q_RH_HFE__)) - ( 0.3735 * cos__q_RH_HAA__));
+ (*this)(3,3) = ((- cos__q_RH_HFE__ * sin__q_RH_KFE__) - ( sin__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(3,4) = (( sin__q_RH_HFE__ * sin__q_RH_KFE__) - ( cos__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(4,3) = ((( sin__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(4,4) = (((- sin__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(4,5) = cos__q_RH_HAA__;
+ (*this)(5,3) = ((( cos__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__) - (( cos__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(5,4) = ((( cos__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) + (( cos__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(5,5) = sin__q_RH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LF_foot_X_fr_trunk::Type_fr_LF_foot_X_fr_trunk()
+{
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LF_foot_X_fr_trunk& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LF_foot_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_LF_KFE__;
+ Scalar sin__q_LF_HFE__;
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+ Scalar cos__q_LF_KFE__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_KFE__ = TRAIT::sin( q(LF_KFE));
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+ cos__q_LF_KFE__ = TRAIT::cos( q(LF_KFE));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,0) = ((- cos__q_LF_HFE__ * sin__q_LF_KFE__) - ( sin__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(0,1) = ((( sin__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( sin__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(0,2) = ((( cos__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( cos__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(0,3) = (((( 0.207 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * sin__q_LF_KFE__) - ((( 0.207 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(0,4) = ((((((- 0.3735 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.08 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.35 * cos__q_LF_HAA__)) * sin__q_LF_KFE__) + (((( 0.08 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.3735 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) * cos__q_LF_KFE__));
+ (*this)(0,5) = (((((( 0.3735 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * cos__q_LF_HFE__)) - ( 0.35 * sin__q_LF_HAA__)) * sin__q_LF_KFE__) + (((( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * sin__q_LF_HFE__) - (( 0.3735 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) * cos__q_LF_KFE__));
+ (*this)(1,0) = (( sin__q_LF_HFE__ * sin__q_LF_KFE__) - ( cos__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(1,1) = ((( sin__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( sin__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,2) = ((( cos__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( cos__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,3) = (((( 0.207 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) + ((( 0.207 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,4) = ((((((- 0.08 * cos__q_LF_HAA__) * sin__q_LF_HFE__) - (( 0.3735 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) * sin__q_LF_KFE__) + (((((- 0.3735 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.08 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.35 * cos__q_LF_HAA__)) * cos__q_LF_KFE__)) + ( 0.33 * cos__q_LF_HAA__));
+ (*this)(1,5) = ((((((( 0.08 * sin__q_LF_HAA__) - 0.207) * sin__q_LF_HFE__) + (( 0.3735 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) * sin__q_LF_KFE__) + ((((( 0.3735 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * cos__q_LF_HFE__)) - ( 0.35 * sin__q_LF_HAA__)) * cos__q_LF_KFE__)) - ( 0.33 * sin__q_LF_HAA__));
+ (*this)(2,1) = cos__q_LF_HAA__;
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(2,3) = ((((((- 0.33 * sin__q_LF_HFE__) * sin__q_LF_KFE__) + (( 0.33 * cos__q_LF_HFE__) * cos__q_LF_KFE__)) + ( 0.35 * cos__q_LF_HFE__)) - ( 0.207 * sin__q_LF_HAA__)) + 0.08);
+ (*this)(2,4) = ((((((- 0.33 * sin__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) - ((( 0.33 * sin__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__)) - (( 0.35 * sin__q_LF_HAA__) * sin__q_LF_HFE__)) + ( 0.3735 * sin__q_LF_HAA__));
+ (*this)(2,5) = ((((((- 0.33 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * sin__q_LF_KFE__) - ((( 0.33 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * cos__q_LF_KFE__)) - (( 0.35 * cos__q_LF_HAA__) * sin__q_LF_HFE__)) + ( 0.3735 * cos__q_LF_HAA__));
+ (*this)(3,3) = ((- cos__q_LF_HFE__ * sin__q_LF_KFE__) - ( sin__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(3,4) = ((( sin__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( sin__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(3,5) = ((( cos__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( cos__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(4,3) = (( sin__q_LF_HFE__ * sin__q_LF_KFE__) - ( cos__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(4,4) = ((( sin__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( sin__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(4,5) = ((( cos__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( cos__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(5,4) = cos__q_LF_HAA__;
+ (*this)(5,5) = - sin__q_LF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RF_foot_X_fr_trunk::Type_fr_RF_foot_X_fr_trunk()
+{
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RF_foot_X_fr_trunk& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RF_foot_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_RF_KFE__;
+ Scalar sin__q_RF_HFE__;
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+ Scalar cos__q_RF_KFE__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_KFE__ = TRAIT::sin( q(RF_KFE));
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+ cos__q_RF_KFE__ = TRAIT::cos( q(RF_KFE));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,0) = ((- cos__q_RF_HFE__ * sin__q_RF_KFE__) - ( sin__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(0,1) = ((( sin__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(0,2) = ((( cos__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__) - (( cos__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(0,3) = (((( 0.207 * cos__q_RF_HAA__) * cos__q_RF_HFE__) * cos__q_RF_KFE__) - ((( 0.207 * cos__q_RF_HAA__) * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(0,4) = ((((((- 0.3735 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.08 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.35 * cos__q_RF_HAA__)) * sin__q_RF_KFE__) + (((( 0.08 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.3735 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) * cos__q_RF_KFE__));
+ (*this)(0,5) = ((((((- 0.3735 * sin__q_RF_HAA__) * sin__q_RF_HFE__) + ((( 0.08 * sin__q_RF_HAA__) - 0.207) * cos__q_RF_HFE__)) + ( 0.35 * sin__q_RF_HAA__)) * sin__q_RF_KFE__) + ((((( 0.08 * sin__q_RF_HAA__) - 0.207) * sin__q_RF_HFE__) + (( 0.3735 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) * cos__q_RF_KFE__));
+ (*this)(1,0) = (( sin__q_RF_HFE__ * sin__q_RF_KFE__) - ( cos__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(1,1) = (((- sin__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(1,2) = ((( cos__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) + (( cos__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(1,3) = ((((- 0.207 * cos__q_RF_HAA__) * cos__q_RF_HFE__) * sin__q_RF_KFE__) - ((( 0.207 * cos__q_RF_HAA__) * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(1,4) = ((((((- 0.08 * cos__q_RF_HAA__) * sin__q_RF_HFE__) - (( 0.3735 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + (((((- 0.3735 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.08 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.35 * cos__q_RF_HAA__)) * cos__q_RF_KFE__)) + ( 0.33 * cos__q_RF_HAA__));
+ (*this)(1,5) = (((((( 0.207 - ( 0.08 * sin__q_RF_HAA__)) * sin__q_RF_HFE__) - (( 0.3735 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + (((((- 0.3735 * sin__q_RF_HAA__) * sin__q_RF_HFE__) + ((( 0.08 * sin__q_RF_HAA__) - 0.207) * cos__q_RF_HFE__)) + ( 0.35 * sin__q_RF_HAA__)) * cos__q_RF_KFE__)) + ( 0.33 * sin__q_RF_HAA__));
+ (*this)(2,1) = cos__q_RF_HAA__;
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(2,3) = ((((((- 0.33 * sin__q_RF_HFE__) * sin__q_RF_KFE__) + (( 0.33 * cos__q_RF_HFE__) * cos__q_RF_KFE__)) + ( 0.35 * cos__q_RF_HFE__)) - ( 0.207 * sin__q_RF_HAA__)) + 0.08);
+ (*this)(2,4) = (((((( 0.33 * sin__q_RF_HAA__) * cos__q_RF_HFE__) * sin__q_RF_KFE__) + ((( 0.33 * sin__q_RF_HAA__) * sin__q_RF_HFE__) * cos__q_RF_KFE__)) + (( 0.35 * sin__q_RF_HAA__) * sin__q_RF_HFE__)) - ( 0.3735 * sin__q_RF_HAA__));
+ (*this)(2,5) = ((((((- 0.33 * cos__q_RF_HAA__) * cos__q_RF_HFE__) * sin__q_RF_KFE__) - ((( 0.33 * cos__q_RF_HAA__) * sin__q_RF_HFE__) * cos__q_RF_KFE__)) - (( 0.35 * cos__q_RF_HAA__) * sin__q_RF_HFE__)) + ( 0.3735 * cos__q_RF_HAA__));
+ (*this)(3,3) = ((- cos__q_RF_HFE__ * sin__q_RF_KFE__) - ( sin__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(3,4) = ((( sin__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(3,5) = ((( cos__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__) - (( cos__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(4,3) = (( sin__q_RF_HFE__ * sin__q_RF_KFE__) - ( cos__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(4,4) = (((- sin__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(4,5) = ((( cos__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) + (( cos__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(5,4) = cos__q_RF_HAA__;
+ (*this)(5,5) = sin__q_RF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LH_foot_X_fr_trunk::Type_fr_LH_foot_X_fr_trunk()
+{
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LH_foot_X_fr_trunk& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LH_foot_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_LH_KFE__;
+ Scalar sin__q_LH_HFE__;
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+ Scalar cos__q_LH_KFE__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_KFE__ = TRAIT::sin( q(LH_KFE));
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+ cos__q_LH_KFE__ = TRAIT::cos( q(LH_KFE));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,0) = ((- cos__q_LH_HFE__ * sin__q_LH_KFE__) - ( sin__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(0,1) = ((( sin__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( sin__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(0,2) = ((( cos__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( cos__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(0,3) = (((( 0.207 * cos__q_LH_HAA__) * sin__q_LH_HFE__) * sin__q_LH_KFE__) - ((( 0.207 * cos__q_LH_HAA__) * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(0,4) = (((((( 0.3735 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.08 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) + ( 0.35 * cos__q_LH_HAA__)) * sin__q_LH_KFE__) + (((( 0.08 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.3735 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) * cos__q_LH_KFE__));
+ (*this)(0,5) = ((((((- 0.3735 * sin__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * cos__q_LH_HFE__)) - ( 0.35 * sin__q_LH_HAA__)) * sin__q_LH_KFE__) + (((( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * sin__q_LH_HFE__) + (( 0.3735 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) * cos__q_LH_KFE__));
+ (*this)(1,0) = (( sin__q_LH_HFE__ * sin__q_LH_KFE__) - ( cos__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(1,1) = ((( sin__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( sin__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,2) = ((( cos__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( cos__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,3) = (((( 0.207 * cos__q_LH_HAA__) * cos__q_LH_HFE__) * sin__q_LH_KFE__) + ((( 0.207 * cos__q_LH_HAA__) * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,4) = (((((( 0.3735 * cos__q_LH_HAA__) * cos__q_LH_HFE__) - (( 0.08 * cos__q_LH_HAA__) * sin__q_LH_HFE__)) * sin__q_LH_KFE__) + ((((( 0.3735 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.08 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) + ( 0.35 * cos__q_LH_HAA__)) * cos__q_LH_KFE__)) + ( 0.33 * cos__q_LH_HAA__));
+ (*this)(1,5) = ((((((( 0.08 * sin__q_LH_HAA__) - 0.207) * sin__q_LH_HFE__) - (( 0.3735 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) * sin__q_LH_KFE__) + (((((- 0.3735 * sin__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * cos__q_LH_HFE__)) - ( 0.35 * sin__q_LH_HAA__)) * cos__q_LH_KFE__)) - ( 0.33 * sin__q_LH_HAA__));
+ (*this)(2,1) = cos__q_LH_HAA__;
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(2,3) = ((((((- 0.33 * sin__q_LH_HFE__) * sin__q_LH_KFE__) + (( 0.33 * cos__q_LH_HFE__) * cos__q_LH_KFE__)) + ( 0.35 * cos__q_LH_HFE__)) - ( 0.207 * sin__q_LH_HAA__)) + 0.08);
+ (*this)(2,4) = ((((((- 0.33 * sin__q_LH_HAA__) * cos__q_LH_HFE__) * sin__q_LH_KFE__) - ((( 0.33 * sin__q_LH_HAA__) * sin__q_LH_HFE__) * cos__q_LH_KFE__)) - (( 0.35 * sin__q_LH_HAA__) * sin__q_LH_HFE__)) - ( 0.3735 * sin__q_LH_HAA__));
+ (*this)(2,5) = ((((((- 0.33 * cos__q_LH_HAA__) * cos__q_LH_HFE__) * sin__q_LH_KFE__) - ((( 0.33 * cos__q_LH_HAA__) * sin__q_LH_HFE__) * cos__q_LH_KFE__)) - (( 0.35 * cos__q_LH_HAA__) * sin__q_LH_HFE__)) - ( 0.3735 * cos__q_LH_HAA__));
+ (*this)(3,3) = ((- cos__q_LH_HFE__ * sin__q_LH_KFE__) - ( sin__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(3,4) = ((( sin__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( sin__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(3,5) = ((( cos__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( cos__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(4,3) = (( sin__q_LH_HFE__ * sin__q_LH_KFE__) - ( cos__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(4,4) = ((( sin__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( sin__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(4,5) = ((( cos__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( cos__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(5,4) = cos__q_LH_HAA__;
+ (*this)(5,5) = - sin__q_LH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RH_foot_X_fr_trunk::Type_fr_RH_foot_X_fr_trunk()
+{
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RH_foot_X_fr_trunk& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RH_foot_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_RH_KFE__;
+ Scalar sin__q_RH_HFE__;
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+ Scalar cos__q_RH_KFE__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_KFE__ = TRAIT::sin( q(RH_KFE));
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+ cos__q_RH_KFE__ = TRAIT::cos( q(RH_KFE));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,0) = ((- cos__q_RH_HFE__ * sin__q_RH_KFE__) - ( sin__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(0,1) = ((( sin__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(0,2) = ((( cos__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__) - (( cos__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(0,3) = (((( 0.207 * cos__q_RH_HAA__) * cos__q_RH_HFE__) * cos__q_RH_KFE__) - ((( 0.207 * cos__q_RH_HAA__) * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(0,4) = (((((( 0.3735 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.08 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.35 * cos__q_RH_HAA__)) * sin__q_RH_KFE__) + (((( 0.08 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.3735 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) * cos__q_RH_KFE__));
+ (*this)(0,5) = (((((( 0.3735 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ((( 0.08 * sin__q_RH_HAA__) - 0.207) * cos__q_RH_HFE__)) + ( 0.35 * sin__q_RH_HAA__)) * sin__q_RH_KFE__) + ((((( 0.08 * sin__q_RH_HAA__) - 0.207) * sin__q_RH_HFE__) - (( 0.3735 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) * cos__q_RH_KFE__));
+ (*this)(1,0) = (( sin__q_RH_HFE__ * sin__q_RH_KFE__) - ( cos__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(1,1) = (((- sin__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(1,2) = ((( cos__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) + (( cos__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(1,3) = ((((- 0.207 * cos__q_RH_HAA__) * cos__q_RH_HFE__) * sin__q_RH_KFE__) - ((( 0.207 * cos__q_RH_HAA__) * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(1,4) = (((((( 0.3735 * cos__q_RH_HAA__) * cos__q_RH_HFE__) - (( 0.08 * cos__q_RH_HAA__) * sin__q_RH_HFE__)) * sin__q_RH_KFE__) + ((((( 0.3735 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.08 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.35 * cos__q_RH_HAA__)) * cos__q_RH_KFE__)) + ( 0.33 * cos__q_RH_HAA__));
+ (*this)(1,5) = (((((( 0.207 - ( 0.08 * sin__q_RH_HAA__)) * sin__q_RH_HFE__) + (( 0.3735 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) * sin__q_RH_KFE__) + ((((( 0.3735 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ((( 0.08 * sin__q_RH_HAA__) - 0.207) * cos__q_RH_HFE__)) + ( 0.35 * sin__q_RH_HAA__)) * cos__q_RH_KFE__)) + ( 0.33 * sin__q_RH_HAA__));
+ (*this)(2,1) = cos__q_RH_HAA__;
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(2,3) = ((((((- 0.33 * sin__q_RH_HFE__) * sin__q_RH_KFE__) + (( 0.33 * cos__q_RH_HFE__) * cos__q_RH_KFE__)) + ( 0.35 * cos__q_RH_HFE__)) - ( 0.207 * sin__q_RH_HAA__)) + 0.08);
+ (*this)(2,4) = (((((( 0.33 * sin__q_RH_HAA__) * cos__q_RH_HFE__) * sin__q_RH_KFE__) + ((( 0.33 * sin__q_RH_HAA__) * sin__q_RH_HFE__) * cos__q_RH_KFE__)) + (( 0.35 * sin__q_RH_HAA__) * sin__q_RH_HFE__)) + ( 0.3735 * sin__q_RH_HAA__));
+ (*this)(2,5) = ((((((- 0.33 * cos__q_RH_HAA__) * cos__q_RH_HFE__) * sin__q_RH_KFE__) - ((( 0.33 * cos__q_RH_HAA__) * sin__q_RH_HFE__) * cos__q_RH_KFE__)) - (( 0.35 * cos__q_RH_HAA__) * sin__q_RH_HFE__)) - ( 0.3735 * cos__q_RH_HAA__));
+ (*this)(3,3) = ((- cos__q_RH_HFE__ * sin__q_RH_KFE__) - ( sin__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(3,4) = ((( sin__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(3,5) = ((( cos__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__) - (( cos__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(4,3) = (( sin__q_RH_HFE__ * sin__q_RH_KFE__) - ( cos__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(4,4) = (((- sin__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(4,5) = ((( cos__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) + (( cos__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(5,4) = cos__q_RH_HAA__;
+ (*this)(5,5) = sin__q_RH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_HAA::Type_fr_trunk_X_fr_LF_HAA()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = - 1.0;
+ (*this)(0,3) = - 0.207;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = - 1.0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0.3735;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = - 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = - 0.3735;
+ (*this)(2,5) = 0.207;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = - 1.0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = - 1.0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = - 1.0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_HAA& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_HAA::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_HAA::Type_fr_trunk_X_fr_RF_HAA()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 1.0;
+ (*this)(0,3) = 0.207;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 1.0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0.3735;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = - 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0.3735;
+ (*this)(2,5) = 0.207;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 1.0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = 1.0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = - 1.0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_HAA& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_HAA::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_HAA::Type_fr_trunk_X_fr_LH_HAA()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = - 1.0;
+ (*this)(0,3) = - 0.207;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = - 1.0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = - 0.3735;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = - 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0.3735;
+ (*this)(2,5) = 0.207;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = - 1.0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = - 1.0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = - 1.0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_HAA& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_HAA::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_HAA::Type_fr_trunk_X_fr_RH_HAA()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 1.0;
+ (*this)(0,3) = 0.207;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 1.0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = - 0.3735;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = - 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = - 0.3735;
+ (*this)(2,5) = 0.207;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = 0;
+ (*this)(3,5) = 1.0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = 1.0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = - 1.0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_HAA& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_HAA::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_HFE::Type_fr_trunk_X_fr_LF_HFE()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = - 1.0;
+ (*this)(0,2) = 0;
+ (*this)(0,4) = 0;
+ (*this)(1,1) = 0;
+ (*this)(2,1) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = - 1.0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,4) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,4) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_HFE& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_HFE::update(const JState& q) {
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,3) = (- 0.207 * cos__q_LF_HAA__);
+ (*this)(0,5) = ( 0.08 - ( 0.207 * sin__q_LF_HAA__));
+ (*this)(1,0) = - sin__q_LF_HAA__;
+ (*this)(1,2) = cos__q_LF_HAA__;
+ (*this)(1,3) = ( 0.3735 * cos__q_LF_HAA__);
+ (*this)(1,4) = ( 0.08 * cos__q_LF_HAA__);
+ (*this)(1,5) = ( 0.3735 * sin__q_LF_HAA__);
+ (*this)(2,0) = - cos__q_LF_HAA__;
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(2,3) = (- 0.3735 * sin__q_LF_HAA__);
+ (*this)(2,4) = ( 0.207 - ( 0.08 * sin__q_LF_HAA__));
+ (*this)(2,5) = ( 0.3735 * cos__q_LF_HAA__);
+ (*this)(4,3) = - sin__q_LF_HAA__;
+ (*this)(4,5) = cos__q_LF_HAA__;
+ (*this)(5,3) = - cos__q_LF_HAA__;
+ (*this)(5,5) = - sin__q_LF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_HFE::Type_fr_trunk_X_fr_RF_HFE()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = - 1.0;
+ (*this)(0,2) = 0;
+ (*this)(0,4) = 0;
+ (*this)(1,1) = 0;
+ (*this)(2,1) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = - 1.0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,4) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,4) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_HFE& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_HFE::update(const JState& q) {
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,3) = ( 0.207 * cos__q_RF_HAA__);
+ (*this)(0,5) = ( 0.08 - ( 0.207 * sin__q_RF_HAA__));
+ (*this)(1,0) = sin__q_RF_HAA__;
+ (*this)(1,2) = cos__q_RF_HAA__;
+ (*this)(1,3) = ( 0.3735 * cos__q_RF_HAA__);
+ (*this)(1,4) = ( 0.08 * cos__q_RF_HAA__);
+ (*this)(1,5) = (- 0.3735 * sin__q_RF_HAA__);
+ (*this)(2,0) = - cos__q_RF_HAA__;
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(2,3) = ( 0.3735 * sin__q_RF_HAA__);
+ (*this)(2,4) = (( 0.08 * sin__q_RF_HAA__) - 0.207);
+ (*this)(2,5) = ( 0.3735 * cos__q_RF_HAA__);
+ (*this)(4,3) = sin__q_RF_HAA__;
+ (*this)(4,5) = cos__q_RF_HAA__;
+ (*this)(5,3) = - cos__q_RF_HAA__;
+ (*this)(5,5) = sin__q_RF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_HFE::Type_fr_trunk_X_fr_LH_HFE()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = - 1.0;
+ (*this)(0,2) = 0;
+ (*this)(0,4) = 0;
+ (*this)(1,1) = 0;
+ (*this)(2,1) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = - 1.0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,4) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,4) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_HFE& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_HFE::update(const JState& q) {
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,3) = (- 0.207 * cos__q_LH_HAA__);
+ (*this)(0,5) = ( 0.08 - ( 0.207 * sin__q_LH_HAA__));
+ (*this)(1,0) = - sin__q_LH_HAA__;
+ (*this)(1,2) = cos__q_LH_HAA__;
+ (*this)(1,3) = (- 0.3735 * cos__q_LH_HAA__);
+ (*this)(1,4) = ( 0.08 * cos__q_LH_HAA__);
+ (*this)(1,5) = (- 0.3735 * sin__q_LH_HAA__);
+ (*this)(2,0) = - cos__q_LH_HAA__;
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(2,3) = ( 0.3735 * sin__q_LH_HAA__);
+ (*this)(2,4) = ( 0.207 - ( 0.08 * sin__q_LH_HAA__));
+ (*this)(2,5) = (- 0.3735 * cos__q_LH_HAA__);
+ (*this)(4,3) = - sin__q_LH_HAA__;
+ (*this)(4,5) = cos__q_LH_HAA__;
+ (*this)(5,3) = - cos__q_LH_HAA__;
+ (*this)(5,5) = - sin__q_LH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_HFE::Type_fr_trunk_X_fr_RH_HFE()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = - 1.0;
+ (*this)(0,2) = 0;
+ (*this)(0,4) = 0;
+ (*this)(1,1) = 0;
+ (*this)(2,1) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 0;
+ (*this)(3,4) = - 1.0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,4) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,4) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_HFE& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_HFE::update(const JState& q) {
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,3) = ( 0.207 * cos__q_RH_HAA__);
+ (*this)(0,5) = ( 0.08 - ( 0.207 * sin__q_RH_HAA__));
+ (*this)(1,0) = sin__q_RH_HAA__;
+ (*this)(1,2) = cos__q_RH_HAA__;
+ (*this)(1,3) = (- 0.3735 * cos__q_RH_HAA__);
+ (*this)(1,4) = ( 0.08 * cos__q_RH_HAA__);
+ (*this)(1,5) = ( 0.3735 * sin__q_RH_HAA__);
+ (*this)(2,0) = - cos__q_RH_HAA__;
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(2,3) = (- 0.3735 * sin__q_RH_HAA__);
+ (*this)(2,4) = (( 0.08 * sin__q_RH_HAA__) - 0.207);
+ (*this)(2,5) = (- 0.3735 * cos__q_RH_HAA__);
+ (*this)(4,3) = sin__q_RH_HAA__;
+ (*this)(4,5) = cos__q_RH_HAA__;
+ (*this)(5,3) = - cos__q_RH_HAA__;
+ (*this)(5,5) = sin__q_RH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_KFE::Type_fr_trunk_X_fr_LF_KFE()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_KFE& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_KFE::update(const JState& q) {
+ Scalar sin__q_LF_HFE__;
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,0) = - sin__q_LF_HFE__;
+ (*this)(0,1) = - cos__q_LF_HFE__;
+ (*this)(0,3) = ((- 0.207 * cos__q_LF_HAA__) * cos__q_LF_HFE__);
+ (*this)(0,4) = (( 0.207 * cos__q_LF_HAA__) * sin__q_LF_HFE__);
+ (*this)(0,5) = ((( 0.35 * cos__q_LF_HFE__) - ( 0.207 * sin__q_LF_HAA__)) + 0.08);
+ (*this)(1,0) = (- sin__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(1,1) = ( sin__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(1,2) = cos__q_LF_HAA__;
+ (*this)(1,3) = ((( 0.08 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.3735 * cos__q_LF_HAA__) * cos__q_LF_HFE__));
+ (*this)(1,4) = ((((- 0.3735 * cos__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.08 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) + ( 0.35 * cos__q_LF_HAA__));
+ (*this)(1,5) = (( 0.3735 * sin__q_LF_HAA__) - (( 0.35 * sin__q_LF_HAA__) * sin__q_LF_HFE__));
+ (*this)(2,0) = (- cos__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(2,1) = ( cos__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(2,3) = ((( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * sin__q_LF_HFE__) - (( 0.3735 * sin__q_LF_HAA__) * cos__q_LF_HFE__));
+ (*this)(2,4) = (((( 0.3735 * sin__q_LF_HAA__) * sin__q_LF_HFE__) + (( 0.207 - ( 0.08 * sin__q_LF_HAA__)) * cos__q_LF_HFE__)) - ( 0.35 * sin__q_LF_HAA__));
+ (*this)(2,5) = (( 0.3735 * cos__q_LF_HAA__) - (( 0.35 * cos__q_LF_HAA__) * sin__q_LF_HFE__));
+ (*this)(3,3) = - sin__q_LF_HFE__;
+ (*this)(3,4) = - cos__q_LF_HFE__;
+ (*this)(4,3) = (- sin__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(4,4) = ( sin__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(4,5) = cos__q_LF_HAA__;
+ (*this)(5,3) = (- cos__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(5,4) = ( cos__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(5,5) = - sin__q_LF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_KFE::Type_fr_trunk_X_fr_RF_KFE()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_KFE& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_KFE::update(const JState& q) {
+ Scalar sin__q_RF_HFE__;
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,0) = - sin__q_RF_HFE__;
+ (*this)(0,1) = - cos__q_RF_HFE__;
+ (*this)(0,3) = (( 0.207 * cos__q_RF_HAA__) * cos__q_RF_HFE__);
+ (*this)(0,4) = ((- 0.207 * cos__q_RF_HAA__) * sin__q_RF_HFE__);
+ (*this)(0,5) = ((( 0.35 * cos__q_RF_HFE__) - ( 0.207 * sin__q_RF_HAA__)) + 0.08);
+ (*this)(1,0) = ( sin__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(1,1) = (- sin__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(1,2) = cos__q_RF_HAA__;
+ (*this)(1,3) = ((( 0.08 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.3735 * cos__q_RF_HAA__) * cos__q_RF_HFE__));
+ (*this)(1,4) = ((((- 0.3735 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.08 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.35 * cos__q_RF_HAA__));
+ (*this)(1,5) = ((( 0.35 * sin__q_RF_HAA__) * sin__q_RF_HFE__) - ( 0.3735 * sin__q_RF_HAA__));
+ (*this)(2,0) = (- cos__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(2,1) = ( cos__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(2,3) = (((( 0.08 * sin__q_RF_HAA__) - 0.207) * sin__q_RF_HFE__) + (( 0.3735 * sin__q_RF_HAA__) * cos__q_RF_HFE__));
+ (*this)(2,4) = ((((- 0.3735 * sin__q_RF_HAA__) * sin__q_RF_HFE__) + ((( 0.08 * sin__q_RF_HAA__) - 0.207) * cos__q_RF_HFE__)) + ( 0.35 * sin__q_RF_HAA__));
+ (*this)(2,5) = (( 0.3735 * cos__q_RF_HAA__) - (( 0.35 * cos__q_RF_HAA__) * sin__q_RF_HFE__));
+ (*this)(3,3) = - sin__q_RF_HFE__;
+ (*this)(3,4) = - cos__q_RF_HFE__;
+ (*this)(4,3) = ( sin__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(4,4) = (- sin__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(4,5) = cos__q_RF_HAA__;
+ (*this)(5,3) = (- cos__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(5,4) = ( cos__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(5,5) = sin__q_RF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_KFE::Type_fr_trunk_X_fr_LH_KFE()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_KFE& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_KFE::update(const JState& q) {
+ Scalar sin__q_LH_HFE__;
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,0) = - sin__q_LH_HFE__;
+ (*this)(0,1) = - cos__q_LH_HFE__;
+ (*this)(0,3) = ((- 0.207 * cos__q_LH_HAA__) * cos__q_LH_HFE__);
+ (*this)(0,4) = (( 0.207 * cos__q_LH_HAA__) * sin__q_LH_HFE__);
+ (*this)(0,5) = ((( 0.35 * cos__q_LH_HFE__) - ( 0.207 * sin__q_LH_HAA__)) + 0.08);
+ (*this)(1,0) = (- sin__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(1,1) = ( sin__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(1,2) = cos__q_LH_HAA__;
+ (*this)(1,3) = ((( 0.08 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.3735 * cos__q_LH_HAA__) * cos__q_LH_HFE__));
+ (*this)(1,4) = (((( 0.3735 * cos__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.08 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) + ( 0.35 * cos__q_LH_HAA__));
+ (*this)(1,5) = (((- 0.35 * sin__q_LH_HAA__) * sin__q_LH_HFE__) - ( 0.3735 * sin__q_LH_HAA__));
+ (*this)(2,0) = (- cos__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(2,1) = ( cos__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(2,3) = ((( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * sin__q_LH_HFE__) + (( 0.3735 * sin__q_LH_HAA__) * cos__q_LH_HFE__));
+ (*this)(2,4) = ((((- 0.3735 * sin__q_LH_HAA__) * sin__q_LH_HFE__) + (( 0.207 - ( 0.08 * sin__q_LH_HAA__)) * cos__q_LH_HFE__)) - ( 0.35 * sin__q_LH_HAA__));
+ (*this)(2,5) = (((- 0.35 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - ( 0.3735 * cos__q_LH_HAA__));
+ (*this)(3,3) = - sin__q_LH_HFE__;
+ (*this)(3,4) = - cos__q_LH_HFE__;
+ (*this)(4,3) = (- sin__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(4,4) = ( sin__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(4,5) = cos__q_LH_HAA__;
+ (*this)(5,3) = (- cos__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(5,4) = ( cos__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(5,5) = - sin__q_LH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_KFE::Type_fr_trunk_X_fr_RH_KFE()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_KFE& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_KFE::update(const JState& q) {
+ Scalar sin__q_RH_HFE__;
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,0) = - sin__q_RH_HFE__;
+ (*this)(0,1) = - cos__q_RH_HFE__;
+ (*this)(0,3) = (( 0.207 * cos__q_RH_HAA__) * cos__q_RH_HFE__);
+ (*this)(0,4) = ((- 0.207 * cos__q_RH_HAA__) * sin__q_RH_HFE__);
+ (*this)(0,5) = ((( 0.35 * cos__q_RH_HFE__) - ( 0.207 * sin__q_RH_HAA__)) + 0.08);
+ (*this)(1,0) = ( sin__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(1,1) = (- sin__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(1,2) = cos__q_RH_HAA__;
+ (*this)(1,3) = ((( 0.08 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.3735 * cos__q_RH_HAA__) * cos__q_RH_HFE__));
+ (*this)(1,4) = (((( 0.3735 * cos__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.08 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.35 * cos__q_RH_HAA__));
+ (*this)(1,5) = ((( 0.35 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ( 0.3735 * sin__q_RH_HAA__));
+ (*this)(2,0) = (- cos__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(2,1) = ( cos__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(2,3) = (((( 0.08 * sin__q_RH_HAA__) - 0.207) * sin__q_RH_HFE__) - (( 0.3735 * sin__q_RH_HAA__) * cos__q_RH_HFE__));
+ (*this)(2,4) = (((( 0.3735 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + ((( 0.08 * sin__q_RH_HAA__) - 0.207) * cos__q_RH_HFE__)) + ( 0.35 * sin__q_RH_HAA__));
+ (*this)(2,5) = (((- 0.35 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - ( 0.3735 * cos__q_RH_HAA__));
+ (*this)(3,3) = - sin__q_RH_HFE__;
+ (*this)(3,4) = - cos__q_RH_HFE__;
+ (*this)(4,3) = ( sin__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(4,4) = (- sin__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(4,5) = cos__q_RH_HAA__;
+ (*this)(5,3) = (- cos__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(5,4) = ( cos__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(5,5) = sin__q_RH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LF_upperleg_X_fr_LF_hipassembly::Type_fr_LF_upperleg_X_fr_LF_hipassembly()
+{
+ (*this)(0,1) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,1) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = - 1;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = - 0.08;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,4) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,4) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = - 1.0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LF_upperleg_X_fr_LF_hipassembly& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LF_upperleg_X_fr_LF_hipassembly::update(const JState& q) {
+ Scalar sin__q_LF_HFE__;
+ Scalar cos__q_LF_HFE__;
+
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+
+ (*this)(0,0) = cos__q_LF_HFE__;
+ (*this)(0,2) = sin__q_LF_HFE__;
+ (*this)(0,4) = (- 0.08 * sin__q_LF_HFE__);
+ (*this)(1,0) = - sin__q_LF_HFE__;
+ (*this)(1,2) = cos__q_LF_HFE__;
+ (*this)(1,4) = (- 0.08 * cos__q_LF_HFE__);
+ (*this)(3,3) = cos__q_LF_HFE__;
+ (*this)(3,5) = sin__q_LF_HFE__;
+ (*this)(4,3) = - sin__q_LF_HFE__;
+ (*this)(4,5) = cos__q_LF_HFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LF_hipassembly_X_fr_LF_upperleg::Type_fr_LF_hipassembly_X_fr_LF_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 0;
+ (*this)(1,2) = - 1.0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = - 0.08;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = 0;
+ (*this)(4,5) = - 1;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LF_hipassembly_X_fr_LF_upperleg& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LF_hipassembly_X_fr_LF_upperleg::update(const JState& q) {
+ Scalar sin__q_LF_HFE__;
+ Scalar cos__q_LF_HFE__;
+
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+
+ (*this)(0,0) = cos__q_LF_HFE__;
+ (*this)(0,1) = - sin__q_LF_HFE__;
+ (*this)(1,3) = (- 0.08 * sin__q_LF_HFE__);
+ (*this)(1,4) = (- 0.08 * cos__q_LF_HFE__);
+ (*this)(2,0) = sin__q_LF_HFE__;
+ (*this)(2,1) = cos__q_LF_HFE__;
+ (*this)(3,3) = cos__q_LF_HFE__;
+ (*this)(3,4) = - sin__q_LF_HFE__;
+ (*this)(5,3) = sin__q_LF_HFE__;
+ (*this)(5,4) = cos__q_LF_HFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LF_lowerleg_X_fr_LF_upperleg::Type_fr_LF_lowerleg_X_fr_LF_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = - 0.35;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LF_lowerleg_X_fr_LF_upperleg& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LF_lowerleg_X_fr_LF_upperleg::update(const JState& q) {
+ Scalar sin__q_LF_KFE__;
+ Scalar cos__q_LF_KFE__;
+
+ sin__q_LF_KFE__ = TRAIT::sin( q(LF_KFE));
+ cos__q_LF_KFE__ = TRAIT::cos( q(LF_KFE));
+
+ (*this)(0,0) = cos__q_LF_KFE__;
+ (*this)(0,1) = sin__q_LF_KFE__;
+ (*this)(0,5) = ( 0.35 * sin__q_LF_KFE__);
+ (*this)(1,0) = - sin__q_LF_KFE__;
+ (*this)(1,1) = cos__q_LF_KFE__;
+ (*this)(1,5) = ( 0.35 * cos__q_LF_KFE__);
+ (*this)(3,3) = cos__q_LF_KFE__;
+ (*this)(3,4) = sin__q_LF_KFE__;
+ (*this)(4,3) = - sin__q_LF_KFE__;
+ (*this)(4,4) = cos__q_LF_KFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LF_upperleg_X_fr_LF_lowerleg::Type_fr_LF_upperleg_X_fr_LF_lowerleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = - 0.35;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1.0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LF_upperleg_X_fr_LF_lowerleg& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LF_upperleg_X_fr_LF_lowerleg::update(const JState& q) {
+ Scalar sin__q_LF_KFE__;
+ Scalar cos__q_LF_KFE__;
+
+ sin__q_LF_KFE__ = TRAIT::sin( q(LF_KFE));
+ cos__q_LF_KFE__ = TRAIT::cos( q(LF_KFE));
+
+ (*this)(0,0) = cos__q_LF_KFE__;
+ (*this)(0,1) = - sin__q_LF_KFE__;
+ (*this)(1,0) = sin__q_LF_KFE__;
+ (*this)(1,1) = cos__q_LF_KFE__;
+ (*this)(2,3) = ( 0.35 * sin__q_LF_KFE__);
+ (*this)(2,4) = ( 0.35 * cos__q_LF_KFE__);
+ (*this)(3,3) = cos__q_LF_KFE__;
+ (*this)(3,4) = - sin__q_LF_KFE__;
+ (*this)(4,3) = sin__q_LF_KFE__;
+ (*this)(4,4) = cos__q_LF_KFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RF_upperleg_X_fr_RF_hipassembly::Type_fr_RF_upperleg_X_fr_RF_hipassembly()
+{
+ (*this)(0,1) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,1) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 1;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0.08;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,4) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,4) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 1.0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RF_upperleg_X_fr_RF_hipassembly& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RF_upperleg_X_fr_RF_hipassembly::update(const JState& q) {
+ Scalar sin__q_RF_HFE__;
+ Scalar cos__q_RF_HFE__;
+
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+
+ (*this)(0,0) = cos__q_RF_HFE__;
+ (*this)(0,2) = - sin__q_RF_HFE__;
+ (*this)(0,4) = ( 0.08 * sin__q_RF_HFE__);
+ (*this)(1,0) = - sin__q_RF_HFE__;
+ (*this)(1,2) = - cos__q_RF_HFE__;
+ (*this)(1,4) = ( 0.08 * cos__q_RF_HFE__);
+ (*this)(3,3) = cos__q_RF_HFE__;
+ (*this)(3,5) = - sin__q_RF_HFE__;
+ (*this)(4,3) = - sin__q_RF_HFE__;
+ (*this)(4,5) = - cos__q_RF_HFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RF_hipassembly_X_fr_RF_upperleg::Type_fr_RF_hipassembly_X_fr_RF_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 0;
+ (*this)(1,2) = 1.0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0.08;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = 0;
+ (*this)(4,5) = 1;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RF_hipassembly_X_fr_RF_upperleg& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RF_hipassembly_X_fr_RF_upperleg::update(const JState& q) {
+ Scalar sin__q_RF_HFE__;
+ Scalar cos__q_RF_HFE__;
+
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+
+ (*this)(0,0) = cos__q_RF_HFE__;
+ (*this)(0,1) = - sin__q_RF_HFE__;
+ (*this)(1,3) = ( 0.08 * sin__q_RF_HFE__);
+ (*this)(1,4) = ( 0.08 * cos__q_RF_HFE__);
+ (*this)(2,0) = - sin__q_RF_HFE__;
+ (*this)(2,1) = - cos__q_RF_HFE__;
+ (*this)(3,3) = cos__q_RF_HFE__;
+ (*this)(3,4) = - sin__q_RF_HFE__;
+ (*this)(5,3) = - sin__q_RF_HFE__;
+ (*this)(5,4) = - cos__q_RF_HFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RF_lowerleg_X_fr_RF_upperleg::Type_fr_RF_lowerleg_X_fr_RF_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = - 0.35;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RF_lowerleg_X_fr_RF_upperleg& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RF_lowerleg_X_fr_RF_upperleg::update(const JState& q) {
+ Scalar sin__q_RF_KFE__;
+ Scalar cos__q_RF_KFE__;
+
+ sin__q_RF_KFE__ = TRAIT::sin( q(RF_KFE));
+ cos__q_RF_KFE__ = TRAIT::cos( q(RF_KFE));
+
+ (*this)(0,0) = cos__q_RF_KFE__;
+ (*this)(0,1) = sin__q_RF_KFE__;
+ (*this)(0,5) = ( 0.35 * sin__q_RF_KFE__);
+ (*this)(1,0) = - sin__q_RF_KFE__;
+ (*this)(1,1) = cos__q_RF_KFE__;
+ (*this)(1,5) = ( 0.35 * cos__q_RF_KFE__);
+ (*this)(3,3) = cos__q_RF_KFE__;
+ (*this)(3,4) = sin__q_RF_KFE__;
+ (*this)(4,3) = - sin__q_RF_KFE__;
+ (*this)(4,4) = cos__q_RF_KFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RF_upperleg_X_fr_RF_lowerleg::Type_fr_RF_upperleg_X_fr_RF_lowerleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = - 0.35;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1.0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RF_upperleg_X_fr_RF_lowerleg& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RF_upperleg_X_fr_RF_lowerleg::update(const JState& q) {
+ Scalar sin__q_RF_KFE__;
+ Scalar cos__q_RF_KFE__;
+
+ sin__q_RF_KFE__ = TRAIT::sin( q(RF_KFE));
+ cos__q_RF_KFE__ = TRAIT::cos( q(RF_KFE));
+
+ (*this)(0,0) = cos__q_RF_KFE__;
+ (*this)(0,1) = - sin__q_RF_KFE__;
+ (*this)(1,0) = sin__q_RF_KFE__;
+ (*this)(1,1) = cos__q_RF_KFE__;
+ (*this)(2,3) = ( 0.35 * sin__q_RF_KFE__);
+ (*this)(2,4) = ( 0.35 * cos__q_RF_KFE__);
+ (*this)(3,3) = cos__q_RF_KFE__;
+ (*this)(3,4) = - sin__q_RF_KFE__;
+ (*this)(4,3) = sin__q_RF_KFE__;
+ (*this)(4,4) = cos__q_RF_KFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LH_upperleg_X_fr_LH_hipassembly::Type_fr_LH_upperleg_X_fr_LH_hipassembly()
+{
+ (*this)(0,1) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,1) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = - 1;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = - 0.08;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,4) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,4) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = - 1.0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LH_upperleg_X_fr_LH_hipassembly& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LH_upperleg_X_fr_LH_hipassembly::update(const JState& q) {
+ Scalar sin__q_LH_HFE__;
+ Scalar cos__q_LH_HFE__;
+
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+
+ (*this)(0,0) = cos__q_LH_HFE__;
+ (*this)(0,2) = sin__q_LH_HFE__;
+ (*this)(0,4) = (- 0.08 * sin__q_LH_HFE__);
+ (*this)(1,0) = - sin__q_LH_HFE__;
+ (*this)(1,2) = cos__q_LH_HFE__;
+ (*this)(1,4) = (- 0.08 * cos__q_LH_HFE__);
+ (*this)(3,3) = cos__q_LH_HFE__;
+ (*this)(3,5) = sin__q_LH_HFE__;
+ (*this)(4,3) = - sin__q_LH_HFE__;
+ (*this)(4,5) = cos__q_LH_HFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LH_hipassembly_X_fr_LH_upperleg::Type_fr_LH_hipassembly_X_fr_LH_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 0;
+ (*this)(1,2) = - 1.0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = - 0.08;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = 0;
+ (*this)(4,5) = - 1;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LH_hipassembly_X_fr_LH_upperleg& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LH_hipassembly_X_fr_LH_upperleg::update(const JState& q) {
+ Scalar sin__q_LH_HFE__;
+ Scalar cos__q_LH_HFE__;
+
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+
+ (*this)(0,0) = cos__q_LH_HFE__;
+ (*this)(0,1) = - sin__q_LH_HFE__;
+ (*this)(1,3) = (- 0.08 * sin__q_LH_HFE__);
+ (*this)(1,4) = (- 0.08 * cos__q_LH_HFE__);
+ (*this)(2,0) = sin__q_LH_HFE__;
+ (*this)(2,1) = cos__q_LH_HFE__;
+ (*this)(3,3) = cos__q_LH_HFE__;
+ (*this)(3,4) = - sin__q_LH_HFE__;
+ (*this)(5,3) = sin__q_LH_HFE__;
+ (*this)(5,4) = cos__q_LH_HFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LH_lowerleg_X_fr_LH_upperleg::Type_fr_LH_lowerleg_X_fr_LH_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = - 0.35;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LH_lowerleg_X_fr_LH_upperleg& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LH_lowerleg_X_fr_LH_upperleg::update(const JState& q) {
+ Scalar sin__q_LH_KFE__;
+ Scalar cos__q_LH_KFE__;
+
+ sin__q_LH_KFE__ = TRAIT::sin( q(LH_KFE));
+ cos__q_LH_KFE__ = TRAIT::cos( q(LH_KFE));
+
+ (*this)(0,0) = cos__q_LH_KFE__;
+ (*this)(0,1) = sin__q_LH_KFE__;
+ (*this)(0,5) = ( 0.35 * sin__q_LH_KFE__);
+ (*this)(1,0) = - sin__q_LH_KFE__;
+ (*this)(1,1) = cos__q_LH_KFE__;
+ (*this)(1,5) = ( 0.35 * cos__q_LH_KFE__);
+ (*this)(3,3) = cos__q_LH_KFE__;
+ (*this)(3,4) = sin__q_LH_KFE__;
+ (*this)(4,3) = - sin__q_LH_KFE__;
+ (*this)(4,4) = cos__q_LH_KFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LH_upperleg_X_fr_LH_lowerleg::Type_fr_LH_upperleg_X_fr_LH_lowerleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = - 0.35;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1.0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LH_upperleg_X_fr_LH_lowerleg& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_LH_upperleg_X_fr_LH_lowerleg::update(const JState& q) {
+ Scalar sin__q_LH_KFE__;
+ Scalar cos__q_LH_KFE__;
+
+ sin__q_LH_KFE__ = TRAIT::sin( q(LH_KFE));
+ cos__q_LH_KFE__ = TRAIT::cos( q(LH_KFE));
+
+ (*this)(0,0) = cos__q_LH_KFE__;
+ (*this)(0,1) = - sin__q_LH_KFE__;
+ (*this)(1,0) = sin__q_LH_KFE__;
+ (*this)(1,1) = cos__q_LH_KFE__;
+ (*this)(2,3) = ( 0.35 * sin__q_LH_KFE__);
+ (*this)(2,4) = ( 0.35 * cos__q_LH_KFE__);
+ (*this)(3,3) = cos__q_LH_KFE__;
+ (*this)(3,4) = - sin__q_LH_KFE__;
+ (*this)(4,3) = sin__q_LH_KFE__;
+ (*this)(4,4) = cos__q_LH_KFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RH_upperleg_X_fr_RH_hipassembly::Type_fr_RH_upperleg_X_fr_RH_hipassembly()
+{
+ (*this)(0,1) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,1) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,5) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 1;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0.08;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,4) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,4) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 1.0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RH_upperleg_X_fr_RH_hipassembly& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RH_upperleg_X_fr_RH_hipassembly::update(const JState& q) {
+ Scalar sin__q_RH_HFE__;
+ Scalar cos__q_RH_HFE__;
+
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+
+ (*this)(0,0) = cos__q_RH_HFE__;
+ (*this)(0,2) = - sin__q_RH_HFE__;
+ (*this)(0,4) = ( 0.08 * sin__q_RH_HFE__);
+ (*this)(1,0) = - sin__q_RH_HFE__;
+ (*this)(1,2) = - cos__q_RH_HFE__;
+ (*this)(1,4) = ( 0.08 * cos__q_RH_HFE__);
+ (*this)(3,3) = cos__q_RH_HFE__;
+ (*this)(3,5) = - sin__q_RH_HFE__;
+ (*this)(4,3) = - sin__q_RH_HFE__;
+ (*this)(4,5) = - cos__q_RH_HFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RH_hipassembly_X_fr_RH_upperleg::Type_fr_RH_hipassembly_X_fr_RH_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 0;
+ (*this)(1,2) = 1.0;
+ (*this)(1,5) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = 0;
+ (*this)(2,5) = 0.08;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,3) = 0;
+ (*this)(4,4) = 0;
+ (*this)(4,5) = 1;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,5) = 0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RH_hipassembly_X_fr_RH_upperleg& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RH_hipassembly_X_fr_RH_upperleg::update(const JState& q) {
+ Scalar sin__q_RH_HFE__;
+ Scalar cos__q_RH_HFE__;
+
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+
+ (*this)(0,0) = cos__q_RH_HFE__;
+ (*this)(0,1) = - sin__q_RH_HFE__;
+ (*this)(1,3) = ( 0.08 * sin__q_RH_HFE__);
+ (*this)(1,4) = ( 0.08 * cos__q_RH_HFE__);
+ (*this)(2,0) = - sin__q_RH_HFE__;
+ (*this)(2,1) = - cos__q_RH_HFE__;
+ (*this)(3,3) = cos__q_RH_HFE__;
+ (*this)(3,4) = - sin__q_RH_HFE__;
+ (*this)(5,3) = - sin__q_RH_HFE__;
+ (*this)(5,4) = - cos__q_RH_HFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RH_lowerleg_X_fr_RH_upperleg::Type_fr_RH_lowerleg_X_fr_RH_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(2,4) = - 0.35;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RH_lowerleg_X_fr_RH_upperleg& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RH_lowerleg_X_fr_RH_upperleg::update(const JState& q) {
+ Scalar sin__q_RH_KFE__;
+ Scalar cos__q_RH_KFE__;
+
+ sin__q_RH_KFE__ = TRAIT::sin( q(RH_KFE));
+ cos__q_RH_KFE__ = TRAIT::cos( q(RH_KFE));
+
+ (*this)(0,0) = cos__q_RH_KFE__;
+ (*this)(0,1) = sin__q_RH_KFE__;
+ (*this)(0,5) = ( 0.35 * sin__q_RH_KFE__);
+ (*this)(1,0) = - sin__q_RH_KFE__;
+ (*this)(1,1) = cos__q_RH_KFE__;
+ (*this)(1,5) = ( 0.35 * cos__q_RH_KFE__);
+ (*this)(3,3) = cos__q_RH_KFE__;
+ (*this)(3,4) = sin__q_RH_KFE__;
+ (*this)(4,3) = - sin__q_RH_KFE__;
+ (*this)(4,4) = cos__q_RH_KFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RH_upperleg_X_fr_RH_lowerleg::Type_fr_RH_upperleg_X_fr_RH_lowerleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0;
+ (*this)(0,4) = 0;
+ (*this)(0,5) = 0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(1,4) = 0;
+ (*this)(1,5) = - 0.35;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1.0;
+ (*this)(2,5) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,5) = 0;
+ (*this)(4,0) = 0;
+ (*this)(4,1) = 0;
+ (*this)(4,2) = 0;
+ (*this)(4,5) = 0;
+ (*this)(5,0) = 0;
+ (*this)(5,1) = 0;
+ (*this)(5,2) = 0;
+ (*this)(5,3) = 0;
+ (*this)(5,4) = 0;
+ (*this)(5,5) = 1;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RH_upperleg_X_fr_RH_lowerleg& iit::TestHyQ::tpl::ForceTransforms<TRAIT>::Type_fr_RH_upperleg_X_fr_RH_lowerleg::update(const JState& q) {
+ Scalar sin__q_RH_KFE__;
+ Scalar cos__q_RH_KFE__;
+
+ sin__q_RH_KFE__ = TRAIT::sin( q(RH_KFE));
+ cos__q_RH_KFE__ = TRAIT::cos( q(RH_KFE));
+
+ (*this)(0,0) = cos__q_RH_KFE__;
+ (*this)(0,1) = - sin__q_RH_KFE__;
+ (*this)(1,0) = sin__q_RH_KFE__;
+ (*this)(1,1) = cos__q_RH_KFE__;
+ (*this)(2,3) = ( 0.35 * sin__q_RH_KFE__);
+ (*this)(2,4) = ( 0.35 * cos__q_RH_KFE__);
+ (*this)(3,3) = cos__q_RH_KFE__;
+ (*this)(3,4) = - sin__q_RH_KFE__;
+ (*this)(4,3) = sin__q_RH_KFE__;
+ (*this)(4,4) = cos__q_RH_KFE__;
+ return *this;
+}
+
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_hipassembly::Type_fr_trunk_X_fr_LF_hipassembly()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = - 1.0;
+ (*this)(0,3) = 0.3735;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0.207;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_hipassembly& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_hipassembly::update(const JState& q) {
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(1,0) = - sin__q_LF_HAA__;
+ (*this)(1,1) = - cos__q_LF_HAA__;
+ (*this)(2,0) = - cos__q_LF_HAA__;
+ (*this)(2,1) = sin__q_LF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_hipassembly::Type_fr_trunk_X_fr_RF_hipassembly()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 1.0;
+ (*this)(0,3) = 0.3735;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = - 0.207;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_hipassembly& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_hipassembly::update(const JState& q) {
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(1,0) = sin__q_RF_HAA__;
+ (*this)(1,1) = cos__q_RF_HAA__;
+ (*this)(2,0) = - cos__q_RF_HAA__;
+ (*this)(2,1) = sin__q_RF_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_hipassembly::Type_fr_trunk_X_fr_LH_hipassembly()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = - 1.0;
+ (*this)(0,3) = - 0.3735;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0.207;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_hipassembly& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_hipassembly::update(const JState& q) {
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(1,0) = - sin__q_LH_HAA__;
+ (*this)(1,1) = - cos__q_LH_HAA__;
+ (*this)(2,0) = - cos__q_LH_HAA__;
+ (*this)(2,1) = sin__q_LH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_hipassembly::Type_fr_trunk_X_fr_RH_hipassembly()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 1.0;
+ (*this)(0,3) = - 0.3735;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = - 0.207;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_hipassembly& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_hipassembly::update(const JState& q) {
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(1,0) = sin__q_RH_HAA__;
+ (*this)(1,1) = cos__q_RH_HAA__;
+ (*this)(2,0) = - cos__q_RH_HAA__;
+ (*this)(2,1) = sin__q_RH_HAA__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_upperleg::Type_fr_trunk_X_fr_LF_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0.3735;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_upperleg& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_upperleg::update(const JState& q) {
+ Scalar sin__q_LF_HFE__;
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,0) = - sin__q_LF_HFE__;
+ (*this)(0,1) = - cos__q_LF_HFE__;
+ (*this)(1,0) = (- sin__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(1,1) = ( sin__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(1,2) = cos__q_LF_HAA__;
+ (*this)(1,3) = ( 0.207 - ( 0.08 * sin__q_LF_HAA__));
+ (*this)(2,0) = (- cos__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(2,1) = ( cos__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(2,3) = (- 0.08 * cos__q_LF_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_upperleg::Type_fr_trunk_X_fr_RF_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0.3735;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_upperleg& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_upperleg::update(const JState& q) {
+ Scalar sin__q_RF_HFE__;
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,0) = - sin__q_RF_HFE__;
+ (*this)(0,1) = - cos__q_RF_HFE__;
+ (*this)(1,0) = ( sin__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(1,1) = (- sin__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(1,2) = cos__q_RF_HAA__;
+ (*this)(1,3) = (( 0.08 * sin__q_RF_HAA__) - 0.207);
+ (*this)(2,0) = (- cos__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(2,1) = ( cos__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(2,3) = (- 0.08 * cos__q_RF_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_upperleg::Type_fr_trunk_X_fr_LH_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = - 0.3735;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_upperleg& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_upperleg::update(const JState& q) {
+ Scalar sin__q_LH_HFE__;
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,0) = - sin__q_LH_HFE__;
+ (*this)(0,1) = - cos__q_LH_HFE__;
+ (*this)(1,0) = (- sin__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(1,1) = ( sin__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(1,2) = cos__q_LH_HAA__;
+ (*this)(1,3) = ( 0.207 - ( 0.08 * sin__q_LH_HAA__));
+ (*this)(2,0) = (- cos__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(2,1) = ( cos__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(2,3) = (- 0.08 * cos__q_LH_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_upperleg::Type_fr_trunk_X_fr_RH_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = - 0.3735;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_upperleg& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_upperleg::update(const JState& q) {
+ Scalar sin__q_RH_HFE__;
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,0) = - sin__q_RH_HFE__;
+ (*this)(0,1) = - cos__q_RH_HFE__;
+ (*this)(1,0) = ( sin__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(1,1) = (- sin__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(1,2) = cos__q_RH_HAA__;
+ (*this)(1,3) = (( 0.08 * sin__q_RH_HAA__) - 0.207);
+ (*this)(2,0) = (- cos__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(2,1) = ( cos__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(2,3) = (- 0.08 * cos__q_RH_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_lowerleg::Type_fr_trunk_X_fr_LF_lowerleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_lowerleg& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_lowerleg::update(const JState& q) {
+ Scalar sin__q_LF_KFE__;
+ Scalar sin__q_LF_HFE__;
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+ Scalar cos__q_LF_KFE__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_KFE__ = TRAIT::sin( q(LF_KFE));
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+ cos__q_LF_KFE__ = TRAIT::cos( q(LF_KFE));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,0) = ((- cos__q_LF_HFE__ * sin__q_LF_KFE__) - ( sin__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(0,1) = (( sin__q_LF_HFE__ * sin__q_LF_KFE__) - ( cos__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(0,3) = ( 0.3735 - ( 0.35 * sin__q_LF_HFE__));
+ (*this)(1,0) = ((( sin__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( sin__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,1) = ((( sin__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( sin__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,2) = cos__q_LF_HAA__;
+ (*this)(1,3) = ((((- 0.35 * sin__q_LF_HAA__) * cos__q_LF_HFE__) - ( 0.08 * sin__q_LF_HAA__)) + 0.207);
+ (*this)(2,0) = ((( cos__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( cos__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(2,1) = ((( cos__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( cos__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(2,3) = (((- 0.35 * cos__q_LF_HAA__) * cos__q_LF_HFE__) - ( 0.08 * cos__q_LF_HAA__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_lowerleg::Type_fr_trunk_X_fr_RF_lowerleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_lowerleg& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_lowerleg::update(const JState& q) {
+ Scalar sin__q_RF_KFE__;
+ Scalar sin__q_RF_HFE__;
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+ Scalar cos__q_RF_KFE__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_KFE__ = TRAIT::sin( q(RF_KFE));
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+ cos__q_RF_KFE__ = TRAIT::cos( q(RF_KFE));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,0) = ((- cos__q_RF_HFE__ * sin__q_RF_KFE__) - ( sin__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(0,1) = (( sin__q_RF_HFE__ * sin__q_RF_KFE__) - ( cos__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(0,3) = ( 0.3735 - ( 0.35 * sin__q_RF_HFE__));
+ (*this)(1,0) = ((( sin__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(1,1) = (((- sin__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(1,2) = cos__q_RF_HAA__;
+ (*this)(1,3) = (((( 0.35 * sin__q_RF_HAA__) * cos__q_RF_HFE__) + ( 0.08 * sin__q_RF_HAA__)) - 0.207);
+ (*this)(2,0) = ((( cos__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__) - (( cos__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(2,1) = ((( cos__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) + (( cos__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(2,3) = (((- 0.35 * cos__q_RF_HAA__) * cos__q_RF_HFE__) - ( 0.08 * cos__q_RF_HAA__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_lowerleg::Type_fr_trunk_X_fr_LH_lowerleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_lowerleg& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_lowerleg::update(const JState& q) {
+ Scalar sin__q_LH_KFE__;
+ Scalar sin__q_LH_HFE__;
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+ Scalar cos__q_LH_KFE__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_KFE__ = TRAIT::sin( q(LH_KFE));
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+ cos__q_LH_KFE__ = TRAIT::cos( q(LH_KFE));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,0) = ((- cos__q_LH_HFE__ * sin__q_LH_KFE__) - ( sin__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(0,1) = (( sin__q_LH_HFE__ * sin__q_LH_KFE__) - ( cos__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(0,3) = ((- 0.35 * sin__q_LH_HFE__) - 0.3735);
+ (*this)(1,0) = ((( sin__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( sin__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,1) = ((( sin__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( sin__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,2) = cos__q_LH_HAA__;
+ (*this)(1,3) = ((((- 0.35 * sin__q_LH_HAA__) * cos__q_LH_HFE__) - ( 0.08 * sin__q_LH_HAA__)) + 0.207);
+ (*this)(2,0) = ((( cos__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( cos__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(2,1) = ((( cos__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( cos__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(2,3) = (((- 0.35 * cos__q_LH_HAA__) * cos__q_LH_HFE__) - ( 0.08 * cos__q_LH_HAA__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_lowerleg::Type_fr_trunk_X_fr_RH_lowerleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_lowerleg& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_lowerleg::update(const JState& q) {
+ Scalar sin__q_RH_KFE__;
+ Scalar sin__q_RH_HFE__;
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+ Scalar cos__q_RH_KFE__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_KFE__ = TRAIT::sin( q(RH_KFE));
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+ cos__q_RH_KFE__ = TRAIT::cos( q(RH_KFE));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,0) = ((- cos__q_RH_HFE__ * sin__q_RH_KFE__) - ( sin__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(0,1) = (( sin__q_RH_HFE__ * sin__q_RH_KFE__) - ( cos__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(0,3) = ((- 0.35 * sin__q_RH_HFE__) - 0.3735);
+ (*this)(1,0) = ((( sin__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(1,1) = (((- sin__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(1,2) = cos__q_RH_HAA__;
+ (*this)(1,3) = (((( 0.35 * sin__q_RH_HAA__) * cos__q_RH_HFE__) + ( 0.08 * sin__q_RH_HAA__)) - 0.207);
+ (*this)(2,0) = ((( cos__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__) - (( cos__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(2,1) = ((( cos__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) + (( cos__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(2,3) = (((- 0.35 * cos__q_RH_HAA__) * cos__q_RH_HFE__) - ( 0.08 * cos__q_RH_HAA__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LF_hipassembly_X_fr_trunk::Type_fr_LF_hipassembly_X_fr_trunk()
+{
+ (*this)(0,0) = 0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = - 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0.3735;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LF_hipassembly_X_fr_trunk& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LF_hipassembly_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,1) = - sin__q_LF_HAA__;
+ (*this)(0,2) = - cos__q_LF_HAA__;
+ (*this)(0,3) = ( 0.207 * sin__q_LF_HAA__);
+ (*this)(1,1) = - cos__q_LF_HAA__;
+ (*this)(1,2) = sin__q_LF_HAA__;
+ (*this)(1,3) = ( 0.207 * cos__q_LF_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RF_hipassembly_X_fr_trunk::Type_fr_RF_hipassembly_X_fr_trunk()
+{
+ (*this)(0,0) = 0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = - 0.3735;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RF_hipassembly_X_fr_trunk& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RF_hipassembly_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,1) = sin__q_RF_HAA__;
+ (*this)(0,2) = - cos__q_RF_HAA__;
+ (*this)(0,3) = ( 0.207 * sin__q_RF_HAA__);
+ (*this)(1,1) = cos__q_RF_HAA__;
+ (*this)(1,2) = sin__q_RF_HAA__;
+ (*this)(1,3) = ( 0.207 * cos__q_RF_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LH_hipassembly_X_fr_trunk::Type_fr_LH_hipassembly_X_fr_trunk()
+{
+ (*this)(0,0) = 0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = - 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = - 0.3735;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LH_hipassembly_X_fr_trunk& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LH_hipassembly_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,1) = - sin__q_LH_HAA__;
+ (*this)(0,2) = - cos__q_LH_HAA__;
+ (*this)(0,3) = ( 0.207 * sin__q_LH_HAA__);
+ (*this)(1,1) = - cos__q_LH_HAA__;
+ (*this)(1,2) = sin__q_LH_HAA__;
+ (*this)(1,3) = ( 0.207 * cos__q_LH_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RH_hipassembly_X_fr_trunk::Type_fr_RH_hipassembly_X_fr_trunk()
+{
+ (*this)(0,0) = 0;
+ (*this)(1,0) = 0;
+ (*this)(2,0) = 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0.3735;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RH_hipassembly_X_fr_trunk& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RH_hipassembly_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,1) = sin__q_RH_HAA__;
+ (*this)(0,2) = - cos__q_RH_HAA__;
+ (*this)(0,3) = ( 0.207 * sin__q_RH_HAA__);
+ (*this)(1,1) = cos__q_RH_HAA__;
+ (*this)(1,2) = sin__q_RH_HAA__;
+ (*this)(1,3) = ( 0.207 * cos__q_RH_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LF_upperleg_X_fr_trunk::Type_fr_LF_upperleg_X_fr_trunk()
+{
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LF_upperleg_X_fr_trunk& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LF_upperleg_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_LF_HFE__;
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,0) = - sin__q_LF_HFE__;
+ (*this)(0,1) = (- sin__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(0,2) = (- cos__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(0,3) = (( 0.3735 * sin__q_LF_HFE__) + ((( 0.207 * sin__q_LF_HAA__) - 0.08) * cos__q_LF_HFE__));
+ (*this)(1,0) = - cos__q_LF_HFE__;
+ (*this)(1,1) = ( sin__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(1,2) = ( cos__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(1,3) = ((( 0.08 - ( 0.207 * sin__q_LF_HAA__)) * sin__q_LF_HFE__) + ( 0.3735 * cos__q_LF_HFE__));
+ (*this)(2,1) = cos__q_LF_HAA__;
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(2,3) = (- 0.207 * cos__q_LF_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RF_upperleg_X_fr_trunk::Type_fr_RF_upperleg_X_fr_trunk()
+{
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RF_upperleg_X_fr_trunk& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RF_upperleg_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_RF_HFE__;
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,0) = - sin__q_RF_HFE__;
+ (*this)(0,1) = ( sin__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(0,2) = (- cos__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(0,3) = (( 0.3735 * sin__q_RF_HFE__) + ((( 0.207 * sin__q_RF_HAA__) - 0.08) * cos__q_RF_HFE__));
+ (*this)(1,0) = - cos__q_RF_HFE__;
+ (*this)(1,1) = (- sin__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(1,2) = ( cos__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(1,3) = ((( 0.08 - ( 0.207 * sin__q_RF_HAA__)) * sin__q_RF_HFE__) + ( 0.3735 * cos__q_RF_HFE__));
+ (*this)(2,1) = cos__q_RF_HAA__;
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(2,3) = ( 0.207 * cos__q_RF_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LH_upperleg_X_fr_trunk::Type_fr_LH_upperleg_X_fr_trunk()
+{
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LH_upperleg_X_fr_trunk& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LH_upperleg_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_LH_HFE__;
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,0) = - sin__q_LH_HFE__;
+ (*this)(0,1) = (- sin__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(0,2) = (- cos__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(0,3) = (((( 0.207 * sin__q_LH_HAA__) - 0.08) * cos__q_LH_HFE__) - ( 0.3735 * sin__q_LH_HFE__));
+ (*this)(1,0) = - cos__q_LH_HFE__;
+ (*this)(1,1) = ( sin__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(1,2) = ( cos__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(1,3) = ((( 0.08 - ( 0.207 * sin__q_LH_HAA__)) * sin__q_LH_HFE__) - ( 0.3735 * cos__q_LH_HFE__));
+ (*this)(2,1) = cos__q_LH_HAA__;
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(2,3) = (- 0.207 * cos__q_LH_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RH_upperleg_X_fr_trunk::Type_fr_RH_upperleg_X_fr_trunk()
+{
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RH_upperleg_X_fr_trunk& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RH_upperleg_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_RH_HFE__;
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,0) = - sin__q_RH_HFE__;
+ (*this)(0,1) = ( sin__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(0,2) = (- cos__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(0,3) = (((( 0.207 * sin__q_RH_HAA__) - 0.08) * cos__q_RH_HFE__) - ( 0.3735 * sin__q_RH_HFE__));
+ (*this)(1,0) = - cos__q_RH_HFE__;
+ (*this)(1,1) = (- sin__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(1,2) = ( cos__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(1,3) = ((( 0.08 - ( 0.207 * sin__q_RH_HAA__)) * sin__q_RH_HFE__) - ( 0.3735 * cos__q_RH_HFE__));
+ (*this)(2,1) = cos__q_RH_HAA__;
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(2,3) = ( 0.207 * cos__q_RH_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LF_lowerleg_X_fr_trunk::Type_fr_LF_lowerleg_X_fr_trunk()
+{
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LF_lowerleg_X_fr_trunk& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LF_lowerleg_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_LF_KFE__;
+ Scalar sin__q_LF_HFE__;
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+ Scalar cos__q_LF_KFE__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_KFE__ = TRAIT::sin( q(LF_KFE));
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+ cos__q_LF_KFE__ = TRAIT::cos( q(LF_KFE));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,0) = ((- cos__q_LF_HFE__ * sin__q_LF_KFE__) - ( sin__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(0,1) = ((( sin__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( sin__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(0,2) = ((( cos__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( cos__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(0,3) = ((((( 0.08 - ( 0.207 * sin__q_LF_HAA__)) * sin__q_LF_HFE__) + ( 0.3735 * cos__q_LF_HFE__)) * sin__q_LF_KFE__) + (((( 0.3735 * sin__q_LF_HFE__) + ((( 0.207 * sin__q_LF_HAA__) - 0.08) * cos__q_LF_HFE__)) - 0.35) * cos__q_LF_KFE__));
+ (*this)(1,0) = (( sin__q_LF_HFE__ * sin__q_LF_KFE__) - ( cos__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(1,1) = ((( sin__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( sin__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,2) = ((( cos__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( cos__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,3) = (((((- 0.3735 * sin__q_LF_HFE__) + (( 0.08 - ( 0.207 * sin__q_LF_HAA__)) * cos__q_LF_HFE__)) + 0.35) * sin__q_LF_KFE__) + (((( 0.08 - ( 0.207 * sin__q_LF_HAA__)) * sin__q_LF_HFE__) + ( 0.3735 * cos__q_LF_HFE__)) * cos__q_LF_KFE__));
+ (*this)(2,1) = cos__q_LF_HAA__;
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(2,3) = (- 0.207 * cos__q_LF_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RF_lowerleg_X_fr_trunk::Type_fr_RF_lowerleg_X_fr_trunk()
+{
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RF_lowerleg_X_fr_trunk& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RF_lowerleg_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_RF_KFE__;
+ Scalar sin__q_RF_HFE__;
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+ Scalar cos__q_RF_KFE__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_KFE__ = TRAIT::sin( q(RF_KFE));
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+ cos__q_RF_KFE__ = TRAIT::cos( q(RF_KFE));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,0) = ((- cos__q_RF_HFE__ * sin__q_RF_KFE__) - ( sin__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(0,1) = ((( sin__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(0,2) = ((( cos__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__) - (( cos__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(0,3) = ((((( 0.08 - ( 0.207 * sin__q_RF_HAA__)) * sin__q_RF_HFE__) + ( 0.3735 * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + (((( 0.3735 * sin__q_RF_HFE__) + ((( 0.207 * sin__q_RF_HAA__) - 0.08) * cos__q_RF_HFE__)) - 0.35) * cos__q_RF_KFE__));
+ (*this)(1,0) = (( sin__q_RF_HFE__ * sin__q_RF_KFE__) - ( cos__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(1,1) = (((- sin__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(1,2) = ((( cos__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) + (( cos__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(1,3) = (((((- 0.3735 * sin__q_RF_HFE__) + (( 0.08 - ( 0.207 * sin__q_RF_HAA__)) * cos__q_RF_HFE__)) + 0.35) * sin__q_RF_KFE__) + (((( 0.08 - ( 0.207 * sin__q_RF_HAA__)) * sin__q_RF_HFE__) + ( 0.3735 * cos__q_RF_HFE__)) * cos__q_RF_KFE__));
+ (*this)(2,1) = cos__q_RF_HAA__;
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(2,3) = ( 0.207 * cos__q_RF_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LH_lowerleg_X_fr_trunk::Type_fr_LH_lowerleg_X_fr_trunk()
+{
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LH_lowerleg_X_fr_trunk& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LH_lowerleg_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_LH_KFE__;
+ Scalar sin__q_LH_HFE__;
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+ Scalar cos__q_LH_KFE__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_KFE__ = TRAIT::sin( q(LH_KFE));
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+ cos__q_LH_KFE__ = TRAIT::cos( q(LH_KFE));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,0) = ((- cos__q_LH_HFE__ * sin__q_LH_KFE__) - ( sin__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(0,1) = ((( sin__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( sin__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(0,2) = ((( cos__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( cos__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(0,3) = ((((( 0.08 - ( 0.207 * sin__q_LH_HAA__)) * sin__q_LH_HFE__) - ( 0.3735 * cos__q_LH_HFE__)) * sin__q_LH_KFE__) + ((((- 0.3735 * sin__q_LH_HFE__) + ((( 0.207 * sin__q_LH_HAA__) - 0.08) * cos__q_LH_HFE__)) - 0.35) * cos__q_LH_KFE__));
+ (*this)(1,0) = (( sin__q_LH_HFE__ * sin__q_LH_KFE__) - ( cos__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(1,1) = ((( sin__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( sin__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,2) = ((( cos__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( cos__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,3) = ((((( 0.3735 * sin__q_LH_HFE__) + (( 0.08 - ( 0.207 * sin__q_LH_HAA__)) * cos__q_LH_HFE__)) + 0.35) * sin__q_LH_KFE__) + (((( 0.08 - ( 0.207 * sin__q_LH_HAA__)) * sin__q_LH_HFE__) - ( 0.3735 * cos__q_LH_HFE__)) * cos__q_LH_KFE__));
+ (*this)(2,1) = cos__q_LH_HAA__;
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(2,3) = (- 0.207 * cos__q_LH_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RH_lowerleg_X_fr_trunk::Type_fr_RH_lowerleg_X_fr_trunk()
+{
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RH_lowerleg_X_fr_trunk& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RH_lowerleg_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_RH_KFE__;
+ Scalar sin__q_RH_HFE__;
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+ Scalar cos__q_RH_KFE__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_KFE__ = TRAIT::sin( q(RH_KFE));
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+ cos__q_RH_KFE__ = TRAIT::cos( q(RH_KFE));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,0) = ((- cos__q_RH_HFE__ * sin__q_RH_KFE__) - ( sin__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(0,1) = ((( sin__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(0,2) = ((( cos__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__) - (( cos__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(0,3) = ((((( 0.08 - ( 0.207 * sin__q_RH_HAA__)) * sin__q_RH_HFE__) - ( 0.3735 * cos__q_RH_HFE__)) * sin__q_RH_KFE__) + ((((- 0.3735 * sin__q_RH_HFE__) + ((( 0.207 * sin__q_RH_HAA__) - 0.08) * cos__q_RH_HFE__)) - 0.35) * cos__q_RH_KFE__));
+ (*this)(1,0) = (( sin__q_RH_HFE__ * sin__q_RH_KFE__) - ( cos__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(1,1) = (((- sin__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(1,2) = ((( cos__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) + (( cos__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(1,3) = ((((( 0.3735 * sin__q_RH_HFE__) + (( 0.08 - ( 0.207 * sin__q_RH_HAA__)) * cos__q_RH_HFE__)) + 0.35) * sin__q_RH_KFE__) + (((( 0.08 - ( 0.207 * sin__q_RH_HAA__)) * sin__q_RH_HFE__) - ( 0.3735 * cos__q_RH_HFE__)) * cos__q_RH_KFE__));
+ (*this)(2,1) = cos__q_RH_HAA__;
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(2,3) = ( 0.207 * cos__q_RH_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_LF_hipassemblyCOM::Type_fr_trunk_X_LF_hipassemblyCOM()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = - 1.0;
+ (*this)(0,3) = 0.2045;
+ (*this)(1,2) = 0;
+ (*this)(2,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_LF_hipassemblyCOM& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_LF_hipassemblyCOM::update(const JState& q) {
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(1,0) = - sin__q_LF_HAA__;
+ (*this)(1,1) = - cos__q_LF_HAA__;
+ (*this)(1,3) = ( 0.207 - ( 0.043 * sin__q_LF_HAA__));
+ (*this)(2,0) = - cos__q_LF_HAA__;
+ (*this)(2,1) = sin__q_LF_HAA__;
+ (*this)(2,3) = (- 0.043 * cos__q_LF_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_RF_hipassemblyCOM::Type_fr_trunk_X_RF_hipassemblyCOM()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 1.0;
+ (*this)(0,3) = 0.2045;
+ (*this)(1,2) = 0;
+ (*this)(2,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_RF_hipassemblyCOM& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_RF_hipassemblyCOM::update(const JState& q) {
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(1,0) = sin__q_RF_HAA__;
+ (*this)(1,1) = cos__q_RF_HAA__;
+ (*this)(1,3) = (( 0.043 * sin__q_RF_HAA__) - 0.207);
+ (*this)(2,0) = - cos__q_RF_HAA__;
+ (*this)(2,1) = sin__q_RF_HAA__;
+ (*this)(2,3) = (- 0.043 * cos__q_RF_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_LH_hipassemblyCOM::Type_fr_trunk_X_LH_hipassemblyCOM()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = - 1.0;
+ (*this)(0,3) = - 0.2045;
+ (*this)(1,2) = 0;
+ (*this)(2,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_LH_hipassemblyCOM& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_LH_hipassemblyCOM::update(const JState& q) {
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(1,0) = - sin__q_LH_HAA__;
+ (*this)(1,1) = - cos__q_LH_HAA__;
+ (*this)(1,3) = ( 0.207 - ( 0.043 * sin__q_LH_HAA__));
+ (*this)(2,0) = - cos__q_LH_HAA__;
+ (*this)(2,1) = sin__q_LH_HAA__;
+ (*this)(2,3) = (- 0.043 * cos__q_LH_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_RH_hipassemblyCOM::Type_fr_trunk_X_RH_hipassemblyCOM()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 1.0;
+ (*this)(0,3) = - 0.2045;
+ (*this)(1,2) = 0;
+ (*this)(2,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_RH_hipassemblyCOM& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_RH_hipassemblyCOM::update(const JState& q) {
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(1,0) = sin__q_RH_HAA__;
+ (*this)(1,1) = cos__q_RH_HAA__;
+ (*this)(1,3) = (( 0.043 * sin__q_RH_HAA__) - 0.207);
+ (*this)(2,0) = - cos__q_RH_HAA__;
+ (*this)(2,1) = sin__q_RH_HAA__;
+ (*this)(2,3) = (- 0.043 * cos__q_RH_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_LF_upperlegCOM::Type_fr_trunk_X_LF_upperlegCOM()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_LF_upperlegCOM& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_LF_upperlegCOM::update(const JState& q) {
+ Scalar sin__q_LF_HFE__;
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,0) = - sin__q_LF_HFE__;
+ (*this)(0,1) = - cos__q_LF_HFE__;
+ (*this)(0,3) = (((- 0.151 * sin__q_LF_HFE__) + ( 0.026 * cos__q_LF_HFE__)) + 0.3735);
+ (*this)(1,0) = (- sin__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(1,1) = ( sin__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(1,2) = cos__q_LF_HAA__;
+ (*this)(1,3) = (((((- 0.026 * sin__q_LF_HAA__) * sin__q_LF_HFE__) - (( 0.151 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) - ( 0.08 * sin__q_LF_HAA__)) + 0.207);
+ (*this)(2,0) = (- cos__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(2,1) = ( cos__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(2,3) = ((((- 0.026 * cos__q_LF_HAA__) * sin__q_LF_HFE__) - (( 0.151 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) - ( 0.08 * cos__q_LF_HAA__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_RF_upperlegCOM::Type_fr_trunk_X_RF_upperlegCOM()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_RF_upperlegCOM& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_RF_upperlegCOM::update(const JState& q) {
+ Scalar sin__q_RF_HFE__;
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,0) = - sin__q_RF_HFE__;
+ (*this)(0,1) = - cos__q_RF_HFE__;
+ (*this)(0,3) = (((- 0.151 * sin__q_RF_HFE__) + ( 0.026 * cos__q_RF_HFE__)) + 0.3735);
+ (*this)(1,0) = ( sin__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(1,1) = (- sin__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(1,2) = cos__q_RF_HAA__;
+ (*this)(1,3) = ((((( 0.026 * sin__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.151 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.08 * sin__q_RF_HAA__)) - 0.207);
+ (*this)(2,0) = (- cos__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(2,1) = ( cos__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(2,3) = ((((- 0.026 * cos__q_RF_HAA__) * sin__q_RF_HFE__) - (( 0.151 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) - ( 0.08 * cos__q_RF_HAA__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_LH_upperlegCOM::Type_fr_trunk_X_LH_upperlegCOM()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_LH_upperlegCOM& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_LH_upperlegCOM::update(const JState& q) {
+ Scalar sin__q_LH_HFE__;
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,0) = - sin__q_LH_HFE__;
+ (*this)(0,1) = - cos__q_LH_HFE__;
+ (*this)(0,3) = (((- 0.151 * sin__q_LH_HFE__) - ( 0.026 * cos__q_LH_HFE__)) - 0.3735);
+ (*this)(1,0) = (- sin__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(1,1) = ( sin__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(1,2) = cos__q_LH_HAA__;
+ (*this)(1,3) = ((((( 0.026 * sin__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.151 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) - ( 0.08 * sin__q_LH_HAA__)) + 0.207);
+ (*this)(2,0) = (- cos__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(2,1) = ( cos__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(2,3) = (((( 0.026 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.151 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) - ( 0.08 * cos__q_LH_HAA__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_RH_upperlegCOM::Type_fr_trunk_X_RH_upperlegCOM()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_RH_upperlegCOM& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_RH_upperlegCOM::update(const JState& q) {
+ Scalar sin__q_RH_HFE__;
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,0) = - sin__q_RH_HFE__;
+ (*this)(0,1) = - cos__q_RH_HFE__;
+ (*this)(0,3) = (((- 0.151 * sin__q_RH_HFE__) - ( 0.026 * cos__q_RH_HFE__)) - 0.3735);
+ (*this)(1,0) = ( sin__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(1,1) = (- sin__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(1,2) = cos__q_RH_HAA__;
+ (*this)(1,3) = (((((- 0.026 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.151 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.08 * sin__q_RH_HAA__)) - 0.207);
+ (*this)(2,0) = (- cos__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(2,1) = ( cos__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(2,3) = (((( 0.026 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.151 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) - ( 0.08 * cos__q_RH_HAA__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_LF_lowerlegCOM::Type_fr_trunk_X_LF_lowerlegCOM()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_LF_lowerlegCOM& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_LF_lowerlegCOM::update(const JState& q) {
+ Scalar sin__q_LF_KFE__;
+ Scalar sin__q_LF_HFE__;
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+ Scalar cos__q_LF_KFE__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_KFE__ = TRAIT::sin( q(LF_KFE));
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+ cos__q_LF_KFE__ = TRAIT::cos( q(LF_KFE));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,0) = ((- cos__q_LF_HFE__ * sin__q_LF_KFE__) - ( sin__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(0,1) = (( sin__q_LF_HFE__ * sin__q_LF_KFE__) - ( cos__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(0,3) = (((((- 0.125 * cos__q_LF_HFE__) * sin__q_LF_KFE__) - (( 0.125 * sin__q_LF_HFE__) * cos__q_LF_KFE__)) - ( 0.35 * sin__q_LF_HFE__)) + 0.3735);
+ (*this)(1,0) = ((( sin__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( sin__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,1) = ((( sin__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( sin__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,2) = cos__q_LF_HAA__;
+ (*this)(1,3) = ((((((( 0.125 * sin__q_LF_HAA__) * sin__q_LF_HFE__) * sin__q_LF_KFE__) - ((( 0.125 * sin__q_LF_HAA__) * cos__q_LF_HFE__) * cos__q_LF_KFE__)) - (( 0.35 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) - ( 0.08 * sin__q_LF_HAA__)) + 0.207);
+ (*this)(2,0) = ((( cos__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( cos__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(2,1) = ((( cos__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( cos__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(2,3) = (((((( 0.125 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * sin__q_LF_KFE__) - ((( 0.125 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * cos__q_LF_KFE__)) - (( 0.35 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) - ( 0.08 * cos__q_LF_HAA__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_RF_lowerlegCOM::Type_fr_trunk_X_RF_lowerlegCOM()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_RF_lowerlegCOM& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_RF_lowerlegCOM::update(const JState& q) {
+ Scalar sin__q_RF_KFE__;
+ Scalar sin__q_RF_HFE__;
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+ Scalar cos__q_RF_KFE__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_KFE__ = TRAIT::sin( q(RF_KFE));
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+ cos__q_RF_KFE__ = TRAIT::cos( q(RF_KFE));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,0) = ((- cos__q_RF_HFE__ * sin__q_RF_KFE__) - ( sin__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(0,1) = (( sin__q_RF_HFE__ * sin__q_RF_KFE__) - ( cos__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(0,3) = (((((( 0.001 * sin__q_RF_HFE__) - ( 0.125 * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + (((- 0.125 * sin__q_RF_HFE__) - ( 0.001 * cos__q_RF_HFE__)) * cos__q_RF_KFE__)) - ( 0.35 * sin__q_RF_HFE__)) + 0.3735);
+ (*this)(1,0) = ((( sin__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(1,1) = (((- sin__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(1,2) = cos__q_RF_HAA__;
+ (*this)(1,3) = ((((((((- 0.125 * sin__q_RF_HAA__) * sin__q_RF_HFE__) - (( 0.001 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + (((( 0.125 * sin__q_RF_HAA__) * cos__q_RF_HFE__) - (( 0.001 * sin__q_RF_HAA__) * sin__q_RF_HFE__)) * cos__q_RF_KFE__)) + (( 0.35 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.08 * sin__q_RF_HAA__)) - 0.207);
+ (*this)(2,0) = ((( cos__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__) - (( cos__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(2,1) = ((( cos__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) + (( cos__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(2,3) = ((((((( 0.125 * cos__q_RF_HAA__) * sin__q_RF_HFE__) + (( 0.001 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + (((( 0.001 * cos__q_RF_HAA__) * sin__q_RF_HFE__) - (( 0.125 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) * cos__q_RF_KFE__)) - (( 0.35 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) - ( 0.08 * cos__q_RF_HAA__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_LH_lowerlegCOM::Type_fr_trunk_X_LH_lowerlegCOM()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_LH_lowerlegCOM& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_LH_lowerlegCOM::update(const JState& q) {
+ Scalar sin__q_LH_KFE__;
+ Scalar sin__q_LH_HFE__;
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+ Scalar cos__q_LH_KFE__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_KFE__ = TRAIT::sin( q(LH_KFE));
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+ cos__q_LH_KFE__ = TRAIT::cos( q(LH_KFE));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,0) = ((- cos__q_LH_HFE__ * sin__q_LH_KFE__) - ( sin__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(0,1) = (( sin__q_LH_HFE__ * sin__q_LH_KFE__) - ( cos__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(0,3) = ((((((- 0.001 * sin__q_LH_HFE__) - ( 0.125 * cos__q_LH_HFE__)) * sin__q_LH_KFE__) + ((( 0.001 * cos__q_LH_HFE__) - ( 0.125 * sin__q_LH_HFE__)) * cos__q_LH_KFE__)) - ( 0.35 * sin__q_LH_HFE__)) - 0.3735);
+ (*this)(1,0) = ((( sin__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( sin__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,1) = ((( sin__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( sin__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,2) = cos__q_LH_HAA__;
+ (*this)(1,3) = (((((((( 0.125 * sin__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.001 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) * sin__q_LH_KFE__) + ((((- 0.001 * sin__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.125 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) * cos__q_LH_KFE__)) - (( 0.35 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) - ( 0.08 * sin__q_LH_HAA__)) + 0.207);
+ (*this)(2,0) = ((( cos__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( cos__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(2,1) = ((( cos__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( cos__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(2,3) = ((((((( 0.125 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.001 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) * sin__q_LH_KFE__) + ((((- 0.001 * cos__q_LH_HAA__) * sin__q_LH_HFE__) - (( 0.125 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) * cos__q_LH_KFE__)) - (( 0.35 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) - ( 0.08 * cos__q_LH_HAA__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_RH_lowerlegCOM::Type_fr_trunk_X_RH_lowerlegCOM()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_RH_lowerlegCOM& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_RH_lowerlegCOM::update(const JState& q) {
+ Scalar sin__q_RH_KFE__;
+ Scalar sin__q_RH_HFE__;
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+ Scalar cos__q_RH_KFE__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_KFE__ = TRAIT::sin( q(RH_KFE));
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+ cos__q_RH_KFE__ = TRAIT::cos( q(RH_KFE));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,0) = ((- cos__q_RH_HFE__ * sin__q_RH_KFE__) - ( sin__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(0,1) = (( sin__q_RH_HFE__ * sin__q_RH_KFE__) - ( cos__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(0,3) = ((((((- 0.001 * sin__q_RH_HFE__) - ( 0.125 * cos__q_RH_HFE__)) * sin__q_RH_KFE__) + ((( 0.001 * cos__q_RH_HFE__) - ( 0.125 * sin__q_RH_HFE__)) * cos__q_RH_KFE__)) - ( 0.35 * sin__q_RH_HFE__)) - 0.3735);
+ (*this)(1,0) = ((( sin__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(1,1) = (((- sin__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(1,2) = cos__q_RH_HAA__;
+ (*this)(1,3) = (((((((( 0.001 * sin__q_RH_HAA__) * cos__q_RH_HFE__) - (( 0.125 * sin__q_RH_HAA__) * sin__q_RH_HFE__)) * sin__q_RH_KFE__) + (((( 0.001 * sin__q_RH_HAA__) * sin__q_RH_HFE__) + (( 0.125 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) * cos__q_RH_KFE__)) + (( 0.35 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.08 * sin__q_RH_HAA__)) - 0.207);
+ (*this)(2,0) = ((( cos__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__) - (( cos__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(2,1) = ((( cos__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) + (( cos__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(2,3) = ((((((( 0.125 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.001 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) * sin__q_RH_KFE__) + ((((- 0.001 * cos__q_RH_HAA__) * sin__q_RH_HFE__) - (( 0.125 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) * cos__q_RH_KFE__)) - (( 0.35 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) - ( 0.08 * cos__q_RH_HAA__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LF_foot_X_fr_LF_lowerleg::Type_fr_LF_foot_X_fr_LF_lowerleg()
+{
+ (*this)(0,0) = 1;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 0;
+ (*this)(0,3) = - 0.33;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 1;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LF_foot_X_fr_LF_lowerleg& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LF_foot_X_fr_LF_lowerleg::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RF_foot_X_fr_RF_lowerleg::Type_fr_RF_foot_X_fr_RF_lowerleg()
+{
+ (*this)(0,0) = 1;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 0;
+ (*this)(0,3) = - 0.33;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 1;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RF_foot_X_fr_RF_lowerleg& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RF_foot_X_fr_RF_lowerleg::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LH_foot_X_fr_LH_lowerleg::Type_fr_LH_foot_X_fr_LH_lowerleg()
+{
+ (*this)(0,0) = 1;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 0;
+ (*this)(0,3) = - 0.33;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 1;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LH_foot_X_fr_LH_lowerleg& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LH_foot_X_fr_LH_lowerleg::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RH_foot_X_fr_RH_lowerleg::Type_fr_RH_foot_X_fr_RH_lowerleg()
+{
+ (*this)(0,0) = 1;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 0;
+ (*this)(0,3) = - 0.33;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 1;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RH_foot_X_fr_RH_lowerleg& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RH_foot_X_fr_RH_lowerleg::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_foot::Type_fr_trunk_X_fr_LF_foot()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_foot& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_foot::update(const JState& q) {
+ Scalar sin__q_LF_KFE__;
+ Scalar sin__q_LF_HFE__;
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+ Scalar cos__q_LF_KFE__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_KFE__ = TRAIT::sin( q(LF_KFE));
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+ cos__q_LF_KFE__ = TRAIT::cos( q(LF_KFE));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,0) = ((- cos__q_LF_HFE__ * sin__q_LF_KFE__) - ( sin__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(0,1) = (( sin__q_LF_HFE__ * sin__q_LF_KFE__) - ( cos__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(0,3) = (((((- 0.33 * cos__q_LF_HFE__) * sin__q_LF_KFE__) - (( 0.33 * sin__q_LF_HFE__) * cos__q_LF_KFE__)) - ( 0.35 * sin__q_LF_HFE__)) + 0.3735);
+ (*this)(1,0) = ((( sin__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( sin__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,1) = ((( sin__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( sin__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,2) = cos__q_LF_HAA__;
+ (*this)(1,3) = ((((((( 0.33 * sin__q_LF_HAA__) * sin__q_LF_HFE__) * sin__q_LF_KFE__) - ((( 0.33 * sin__q_LF_HAA__) * cos__q_LF_HFE__) * cos__q_LF_KFE__)) - (( 0.35 * sin__q_LF_HAA__) * cos__q_LF_HFE__)) - ( 0.08 * sin__q_LF_HAA__)) + 0.207);
+ (*this)(2,0) = ((( cos__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( cos__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(2,1) = ((( cos__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( cos__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(2,3) = (((((( 0.33 * cos__q_LF_HAA__) * sin__q_LF_HFE__) * sin__q_LF_KFE__) - ((( 0.33 * cos__q_LF_HAA__) * cos__q_LF_HFE__) * cos__q_LF_KFE__)) - (( 0.35 * cos__q_LF_HAA__) * cos__q_LF_HFE__)) - ( 0.08 * cos__q_LF_HAA__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_foot::Type_fr_trunk_X_fr_RF_foot()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_foot& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_foot::update(const JState& q) {
+ Scalar sin__q_RF_KFE__;
+ Scalar sin__q_RF_HFE__;
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+ Scalar cos__q_RF_KFE__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_KFE__ = TRAIT::sin( q(RF_KFE));
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+ cos__q_RF_KFE__ = TRAIT::cos( q(RF_KFE));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,0) = ((- cos__q_RF_HFE__ * sin__q_RF_KFE__) - ( sin__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(0,1) = (( sin__q_RF_HFE__ * sin__q_RF_KFE__) - ( cos__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(0,3) = (((((- 0.33 * cos__q_RF_HFE__) * sin__q_RF_KFE__) - (( 0.33 * sin__q_RF_HFE__) * cos__q_RF_KFE__)) - ( 0.35 * sin__q_RF_HFE__)) + 0.3735);
+ (*this)(1,0) = ((( sin__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(1,1) = (((- sin__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(1,2) = cos__q_RF_HAA__;
+ (*this)(1,3) = (((((((- 0.33 * sin__q_RF_HAA__) * sin__q_RF_HFE__) * sin__q_RF_KFE__) + ((( 0.33 * sin__q_RF_HAA__) * cos__q_RF_HFE__) * cos__q_RF_KFE__)) + (( 0.35 * sin__q_RF_HAA__) * cos__q_RF_HFE__)) + ( 0.08 * sin__q_RF_HAA__)) - 0.207);
+ (*this)(2,0) = ((( cos__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__) - (( cos__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(2,1) = ((( cos__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) + (( cos__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(2,3) = (((((( 0.33 * cos__q_RF_HAA__) * sin__q_RF_HFE__) * sin__q_RF_KFE__) - ((( 0.33 * cos__q_RF_HAA__) * cos__q_RF_HFE__) * cos__q_RF_KFE__)) - (( 0.35 * cos__q_RF_HAA__) * cos__q_RF_HFE__)) - ( 0.08 * cos__q_RF_HAA__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_foot::Type_fr_trunk_X_fr_LH_foot()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_foot& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_foot::update(const JState& q) {
+ Scalar sin__q_LH_KFE__;
+ Scalar sin__q_LH_HFE__;
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+ Scalar cos__q_LH_KFE__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_KFE__ = TRAIT::sin( q(LH_KFE));
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+ cos__q_LH_KFE__ = TRAIT::cos( q(LH_KFE));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,0) = ((- cos__q_LH_HFE__ * sin__q_LH_KFE__) - ( sin__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(0,1) = (( sin__q_LH_HFE__ * sin__q_LH_KFE__) - ( cos__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(0,3) = (((((- 0.33 * cos__q_LH_HFE__) * sin__q_LH_KFE__) - (( 0.33 * sin__q_LH_HFE__) * cos__q_LH_KFE__)) - ( 0.35 * sin__q_LH_HFE__)) - 0.3735);
+ (*this)(1,0) = ((( sin__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( sin__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,1) = ((( sin__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( sin__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,2) = cos__q_LH_HAA__;
+ (*this)(1,3) = ((((((( 0.33 * sin__q_LH_HAA__) * sin__q_LH_HFE__) * sin__q_LH_KFE__) - ((( 0.33 * sin__q_LH_HAA__) * cos__q_LH_HFE__) * cos__q_LH_KFE__)) - (( 0.35 * sin__q_LH_HAA__) * cos__q_LH_HFE__)) - ( 0.08 * sin__q_LH_HAA__)) + 0.207);
+ (*this)(2,0) = ((( cos__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( cos__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(2,1) = ((( cos__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( cos__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(2,3) = (((((( 0.33 * cos__q_LH_HAA__) * sin__q_LH_HFE__) * sin__q_LH_KFE__) - ((( 0.33 * cos__q_LH_HAA__) * cos__q_LH_HFE__) * cos__q_LH_KFE__)) - (( 0.35 * cos__q_LH_HAA__) * cos__q_LH_HFE__)) - ( 0.08 * cos__q_LH_HAA__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_foot::Type_fr_trunk_X_fr_RH_foot()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_foot& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_foot::update(const JState& q) {
+ Scalar sin__q_RH_KFE__;
+ Scalar sin__q_RH_HFE__;
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+ Scalar cos__q_RH_KFE__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_KFE__ = TRAIT::sin( q(RH_KFE));
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+ cos__q_RH_KFE__ = TRAIT::cos( q(RH_KFE));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,0) = ((- cos__q_RH_HFE__ * sin__q_RH_KFE__) - ( sin__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(0,1) = (( sin__q_RH_HFE__ * sin__q_RH_KFE__) - ( cos__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(0,3) = (((((- 0.33 * cos__q_RH_HFE__) * sin__q_RH_KFE__) - (( 0.33 * sin__q_RH_HFE__) * cos__q_RH_KFE__)) - ( 0.35 * sin__q_RH_HFE__)) - 0.3735);
+ (*this)(1,0) = ((( sin__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(1,1) = (((- sin__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(1,2) = cos__q_RH_HAA__;
+ (*this)(1,3) = (((((((- 0.33 * sin__q_RH_HAA__) * sin__q_RH_HFE__) * sin__q_RH_KFE__) + ((( 0.33 * sin__q_RH_HAA__) * cos__q_RH_HFE__) * cos__q_RH_KFE__)) + (( 0.35 * sin__q_RH_HAA__) * cos__q_RH_HFE__)) + ( 0.08 * sin__q_RH_HAA__)) - 0.207);
+ (*this)(2,0) = ((( cos__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__) - (( cos__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(2,1) = ((( cos__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) + (( cos__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(2,3) = (((((( 0.33 * cos__q_RH_HAA__) * sin__q_RH_HFE__) * sin__q_RH_KFE__) - ((( 0.33 * cos__q_RH_HAA__) * cos__q_RH_HFE__) * cos__q_RH_KFE__)) - (( 0.35 * cos__q_RH_HAA__) * cos__q_RH_HFE__)) - ( 0.08 * cos__q_RH_HAA__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LF_foot_X_fr_trunk::Type_fr_LF_foot_X_fr_trunk()
+{
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LF_foot_X_fr_trunk& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LF_foot_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_LF_KFE__;
+ Scalar sin__q_LF_HFE__;
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+ Scalar cos__q_LF_KFE__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_KFE__ = TRAIT::sin( q(LF_KFE));
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+ cos__q_LF_KFE__ = TRAIT::cos( q(LF_KFE));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,0) = ((- cos__q_LF_HFE__ * sin__q_LF_KFE__) - ( sin__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(0,1) = ((( sin__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( sin__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(0,2) = ((( cos__q_LF_HAA__ * sin__q_LF_HFE__) * sin__q_LF_KFE__) - (( cos__q_LF_HAA__ * cos__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(0,3) = (((((( 0.08 - ( 0.207 * sin__q_LF_HAA__)) * sin__q_LF_HFE__) + ( 0.3735 * cos__q_LF_HFE__)) * sin__q_LF_KFE__) + (((( 0.3735 * sin__q_LF_HFE__) + ((( 0.207 * sin__q_LF_HAA__) - 0.08) * cos__q_LF_HFE__)) - 0.35) * cos__q_LF_KFE__)) - 0.33);
+ (*this)(1,0) = (( sin__q_LF_HFE__ * sin__q_LF_KFE__) - ( cos__q_LF_HFE__ * cos__q_LF_KFE__));
+ (*this)(1,1) = ((( sin__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( sin__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,2) = ((( cos__q_LF_HAA__ * cos__q_LF_HFE__) * sin__q_LF_KFE__) + (( cos__q_LF_HAA__ * sin__q_LF_HFE__) * cos__q_LF_KFE__));
+ (*this)(1,3) = (((((- 0.3735 * sin__q_LF_HFE__) + (( 0.08 - ( 0.207 * sin__q_LF_HAA__)) * cos__q_LF_HFE__)) + 0.35) * sin__q_LF_KFE__) + (((( 0.08 - ( 0.207 * sin__q_LF_HAA__)) * sin__q_LF_HFE__) + ( 0.3735 * cos__q_LF_HFE__)) * cos__q_LF_KFE__));
+ (*this)(2,1) = cos__q_LF_HAA__;
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(2,3) = (- 0.207 * cos__q_LF_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RF_foot_X_fr_trunk::Type_fr_RF_foot_X_fr_trunk()
+{
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RF_foot_X_fr_trunk& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RF_foot_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_RF_KFE__;
+ Scalar sin__q_RF_HFE__;
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+ Scalar cos__q_RF_KFE__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_KFE__ = TRAIT::sin( q(RF_KFE));
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+ cos__q_RF_KFE__ = TRAIT::cos( q(RF_KFE));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,0) = ((- cos__q_RF_HFE__ * sin__q_RF_KFE__) - ( sin__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(0,1) = ((( sin__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__));
+ (*this)(0,2) = ((( cos__q_RF_HAA__ * sin__q_RF_HFE__) * sin__q_RF_KFE__) - (( cos__q_RF_HAA__ * cos__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(0,3) = (((((( 0.08 - ( 0.207 * sin__q_RF_HAA__)) * sin__q_RF_HFE__) + ( 0.3735 * cos__q_RF_HFE__)) * sin__q_RF_KFE__) + (((( 0.3735 * sin__q_RF_HFE__) + ((( 0.207 * sin__q_RF_HAA__) - 0.08) * cos__q_RF_HFE__)) - 0.35) * cos__q_RF_KFE__)) - 0.33);
+ (*this)(1,0) = (( sin__q_RF_HFE__ * sin__q_RF_KFE__) - ( cos__q_RF_HFE__ * cos__q_RF_KFE__));
+ (*this)(1,1) = (((- sin__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) - (( sin__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(1,2) = ((( cos__q_RF_HAA__ * cos__q_RF_HFE__) * sin__q_RF_KFE__) + (( cos__q_RF_HAA__ * sin__q_RF_HFE__) * cos__q_RF_KFE__));
+ (*this)(1,3) = (((((- 0.3735 * sin__q_RF_HFE__) + (( 0.08 - ( 0.207 * sin__q_RF_HAA__)) * cos__q_RF_HFE__)) + 0.35) * sin__q_RF_KFE__) + (((( 0.08 - ( 0.207 * sin__q_RF_HAA__)) * sin__q_RF_HFE__) + ( 0.3735 * cos__q_RF_HFE__)) * cos__q_RF_KFE__));
+ (*this)(2,1) = cos__q_RF_HAA__;
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(2,3) = ( 0.207 * cos__q_RF_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LH_foot_X_fr_trunk::Type_fr_LH_foot_X_fr_trunk()
+{
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LH_foot_X_fr_trunk& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LH_foot_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_LH_KFE__;
+ Scalar sin__q_LH_HFE__;
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+ Scalar cos__q_LH_KFE__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_KFE__ = TRAIT::sin( q(LH_KFE));
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+ cos__q_LH_KFE__ = TRAIT::cos( q(LH_KFE));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,0) = ((- cos__q_LH_HFE__ * sin__q_LH_KFE__) - ( sin__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(0,1) = ((( sin__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( sin__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(0,2) = ((( cos__q_LH_HAA__ * sin__q_LH_HFE__) * sin__q_LH_KFE__) - (( cos__q_LH_HAA__ * cos__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(0,3) = (((((( 0.08 - ( 0.207 * sin__q_LH_HAA__)) * sin__q_LH_HFE__) - ( 0.3735 * cos__q_LH_HFE__)) * sin__q_LH_KFE__) + ((((- 0.3735 * sin__q_LH_HFE__) + ((( 0.207 * sin__q_LH_HAA__) - 0.08) * cos__q_LH_HFE__)) - 0.35) * cos__q_LH_KFE__)) - 0.33);
+ (*this)(1,0) = (( sin__q_LH_HFE__ * sin__q_LH_KFE__) - ( cos__q_LH_HFE__ * cos__q_LH_KFE__));
+ (*this)(1,1) = ((( sin__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( sin__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,2) = ((( cos__q_LH_HAA__ * cos__q_LH_HFE__) * sin__q_LH_KFE__) + (( cos__q_LH_HAA__ * sin__q_LH_HFE__) * cos__q_LH_KFE__));
+ (*this)(1,3) = ((((( 0.3735 * sin__q_LH_HFE__) + (( 0.08 - ( 0.207 * sin__q_LH_HAA__)) * cos__q_LH_HFE__)) + 0.35) * sin__q_LH_KFE__) + (((( 0.08 - ( 0.207 * sin__q_LH_HAA__)) * sin__q_LH_HFE__) - ( 0.3735 * cos__q_LH_HFE__)) * cos__q_LH_KFE__));
+ (*this)(2,1) = cos__q_LH_HAA__;
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(2,3) = (- 0.207 * cos__q_LH_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RH_foot_X_fr_trunk::Type_fr_RH_foot_X_fr_trunk()
+{
+ (*this)(2,0) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RH_foot_X_fr_trunk& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RH_foot_X_fr_trunk::update(const JState& q) {
+ Scalar sin__q_RH_KFE__;
+ Scalar sin__q_RH_HFE__;
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+ Scalar cos__q_RH_KFE__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_KFE__ = TRAIT::sin( q(RH_KFE));
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+ cos__q_RH_KFE__ = TRAIT::cos( q(RH_KFE));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,0) = ((- cos__q_RH_HFE__ * sin__q_RH_KFE__) - ( sin__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(0,1) = ((( sin__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__));
+ (*this)(0,2) = ((( cos__q_RH_HAA__ * sin__q_RH_HFE__) * sin__q_RH_KFE__) - (( cos__q_RH_HAA__ * cos__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(0,3) = (((((( 0.08 - ( 0.207 * sin__q_RH_HAA__)) * sin__q_RH_HFE__) - ( 0.3735 * cos__q_RH_HFE__)) * sin__q_RH_KFE__) + ((((- 0.3735 * sin__q_RH_HFE__) + ((( 0.207 * sin__q_RH_HAA__) - 0.08) * cos__q_RH_HFE__)) - 0.35) * cos__q_RH_KFE__)) - 0.33);
+ (*this)(1,0) = (( sin__q_RH_HFE__ * sin__q_RH_KFE__) - ( cos__q_RH_HFE__ * cos__q_RH_KFE__));
+ (*this)(1,1) = (((- sin__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) - (( sin__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(1,2) = ((( cos__q_RH_HAA__ * cos__q_RH_HFE__) * sin__q_RH_KFE__) + (( cos__q_RH_HAA__ * sin__q_RH_HFE__) * cos__q_RH_KFE__));
+ (*this)(1,3) = ((((( 0.3735 * sin__q_RH_HFE__) + (( 0.08 - ( 0.207 * sin__q_RH_HAA__)) * cos__q_RH_HFE__)) + 0.35) * sin__q_RH_KFE__) + (((( 0.08 - ( 0.207 * sin__q_RH_HAA__)) * sin__q_RH_HFE__) - ( 0.3735 * cos__q_RH_HFE__)) * cos__q_RH_KFE__));
+ (*this)(2,1) = cos__q_RH_HAA__;
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(2,3) = ( 0.207 * cos__q_RH_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_HAA::Type_fr_trunk_X_fr_LF_HAA()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = - 1.0;
+ (*this)(0,3) = 0.3735;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = - 1.0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0.207;
+ (*this)(2,0) = - 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_HAA& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_HAA::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_HAA::Type_fr_trunk_X_fr_RF_HAA()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 1.0;
+ (*this)(0,3) = 0.3735;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 1.0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = - 0.207;
+ (*this)(2,0) = - 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_HAA& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_HAA::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_HAA::Type_fr_trunk_X_fr_LH_HAA()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = - 1.0;
+ (*this)(0,3) = - 0.3735;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = - 1.0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0.207;
+ (*this)(2,0) = - 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_HAA& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_HAA::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_HAA::Type_fr_trunk_X_fr_RH_HAA()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = 0;
+ (*this)(0,2) = 1.0;
+ (*this)(0,3) = - 0.3735;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 1.0;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = - 0.207;
+ (*this)(2,0) = - 1.0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_HAA& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_HAA::update(const JState& q) {
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_HFE::Type_fr_trunk_X_fr_LF_HFE()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = - 1.0;
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0.3735;
+ (*this)(1,1) = 0;
+ (*this)(2,1) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_HFE& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_HFE::update(const JState& q) {
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(1,0) = - sin__q_LF_HAA__;
+ (*this)(1,2) = cos__q_LF_HAA__;
+ (*this)(1,3) = ( 0.207 - ( 0.08 * sin__q_LF_HAA__));
+ (*this)(2,0) = - cos__q_LF_HAA__;
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(2,3) = (- 0.08 * cos__q_LF_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_HFE::Type_fr_trunk_X_fr_RF_HFE()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = - 1.0;
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0.3735;
+ (*this)(1,1) = 0;
+ (*this)(2,1) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_HFE& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_HFE::update(const JState& q) {
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(1,0) = sin__q_RF_HAA__;
+ (*this)(1,2) = cos__q_RF_HAA__;
+ (*this)(1,3) = (( 0.08 * sin__q_RF_HAA__) - 0.207);
+ (*this)(2,0) = - cos__q_RF_HAA__;
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(2,3) = (- 0.08 * cos__q_RF_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_HFE::Type_fr_trunk_X_fr_LH_HFE()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = - 1.0;
+ (*this)(0,2) = 0;
+ (*this)(0,3) = - 0.3735;
+ (*this)(1,1) = 0;
+ (*this)(2,1) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_HFE& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_HFE::update(const JState& q) {
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(1,0) = - sin__q_LH_HAA__;
+ (*this)(1,2) = cos__q_LH_HAA__;
+ (*this)(1,3) = ( 0.207 - ( 0.08 * sin__q_LH_HAA__));
+ (*this)(2,0) = - cos__q_LH_HAA__;
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(2,3) = (- 0.08 * cos__q_LH_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_HFE::Type_fr_trunk_X_fr_RH_HFE()
+{
+ (*this)(0,0) = 0;
+ (*this)(0,1) = - 1.0;
+ (*this)(0,2) = 0;
+ (*this)(0,3) = - 0.3735;
+ (*this)(1,1) = 0;
+ (*this)(2,1) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_HFE& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_HFE::update(const JState& q) {
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(1,0) = sin__q_RH_HAA__;
+ (*this)(1,2) = cos__q_RH_HAA__;
+ (*this)(1,3) = (( 0.08 * sin__q_RH_HAA__) - 0.207);
+ (*this)(2,0) = - cos__q_RH_HAA__;
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(2,3) = (- 0.08 * cos__q_RH_HAA__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_KFE::Type_fr_trunk_X_fr_LF_KFE()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_KFE& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LF_KFE::update(const JState& q) {
+ Scalar sin__q_LF_HFE__;
+ Scalar sin__q_LF_HAA__;
+ Scalar cos__q_LF_HFE__;
+ Scalar cos__q_LF_HAA__;
+
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ sin__q_LF_HAA__ = TRAIT::sin( q(LF_HAA));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+ cos__q_LF_HAA__ = TRAIT::cos( q(LF_HAA));
+
+ (*this)(0,0) = - sin__q_LF_HFE__;
+ (*this)(0,1) = - cos__q_LF_HFE__;
+ (*this)(0,3) = ( 0.3735 - ( 0.35 * sin__q_LF_HFE__));
+ (*this)(1,0) = (- sin__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(1,1) = ( sin__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(1,2) = cos__q_LF_HAA__;
+ (*this)(1,3) = ((((- 0.35 * sin__q_LF_HAA__) * cos__q_LF_HFE__) - ( 0.08 * sin__q_LF_HAA__)) + 0.207);
+ (*this)(2,0) = (- cos__q_LF_HAA__ * cos__q_LF_HFE__);
+ (*this)(2,1) = ( cos__q_LF_HAA__ * sin__q_LF_HFE__);
+ (*this)(2,2) = - sin__q_LF_HAA__;
+ (*this)(2,3) = (((- 0.35 * cos__q_LF_HAA__) * cos__q_LF_HFE__) - ( 0.08 * cos__q_LF_HAA__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_KFE::Type_fr_trunk_X_fr_RF_KFE()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_KFE& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RF_KFE::update(const JState& q) {
+ Scalar sin__q_RF_HFE__;
+ Scalar sin__q_RF_HAA__;
+ Scalar cos__q_RF_HFE__;
+ Scalar cos__q_RF_HAA__;
+
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ sin__q_RF_HAA__ = TRAIT::sin( q(RF_HAA));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+ cos__q_RF_HAA__ = TRAIT::cos( q(RF_HAA));
+
+ (*this)(0,0) = - sin__q_RF_HFE__;
+ (*this)(0,1) = - cos__q_RF_HFE__;
+ (*this)(0,3) = ( 0.3735 - ( 0.35 * sin__q_RF_HFE__));
+ (*this)(1,0) = ( sin__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(1,1) = (- sin__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(1,2) = cos__q_RF_HAA__;
+ (*this)(1,3) = (((( 0.35 * sin__q_RF_HAA__) * cos__q_RF_HFE__) + ( 0.08 * sin__q_RF_HAA__)) - 0.207);
+ (*this)(2,0) = (- cos__q_RF_HAA__ * cos__q_RF_HFE__);
+ (*this)(2,1) = ( cos__q_RF_HAA__ * sin__q_RF_HFE__);
+ (*this)(2,2) = sin__q_RF_HAA__;
+ (*this)(2,3) = (((- 0.35 * cos__q_RF_HAA__) * cos__q_RF_HFE__) - ( 0.08 * cos__q_RF_HAA__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_KFE::Type_fr_trunk_X_fr_LH_KFE()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_KFE& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_LH_KFE::update(const JState& q) {
+ Scalar sin__q_LH_HFE__;
+ Scalar sin__q_LH_HAA__;
+ Scalar cos__q_LH_HFE__;
+ Scalar cos__q_LH_HAA__;
+
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ sin__q_LH_HAA__ = TRAIT::sin( q(LH_HAA));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+ cos__q_LH_HAA__ = TRAIT::cos( q(LH_HAA));
+
+ (*this)(0,0) = - sin__q_LH_HFE__;
+ (*this)(0,1) = - cos__q_LH_HFE__;
+ (*this)(0,3) = ((- 0.35 * sin__q_LH_HFE__) - 0.3735);
+ (*this)(1,0) = (- sin__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(1,1) = ( sin__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(1,2) = cos__q_LH_HAA__;
+ (*this)(1,3) = ((((- 0.35 * sin__q_LH_HAA__) * cos__q_LH_HFE__) - ( 0.08 * sin__q_LH_HAA__)) + 0.207);
+ (*this)(2,0) = (- cos__q_LH_HAA__ * cos__q_LH_HFE__);
+ (*this)(2,1) = ( cos__q_LH_HAA__ * sin__q_LH_HFE__);
+ (*this)(2,2) = - sin__q_LH_HAA__;
+ (*this)(2,3) = (((- 0.35 * cos__q_LH_HAA__) * cos__q_LH_HFE__) - ( 0.08 * cos__q_LH_HAA__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_KFE::Type_fr_trunk_X_fr_RH_KFE()
+{
+ (*this)(0,2) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_KFE& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_trunk_X_fr_RH_KFE::update(const JState& q) {
+ Scalar sin__q_RH_HFE__;
+ Scalar sin__q_RH_HAA__;
+ Scalar cos__q_RH_HFE__;
+ Scalar cos__q_RH_HAA__;
+
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ sin__q_RH_HAA__ = TRAIT::sin( q(RH_HAA));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+ cos__q_RH_HAA__ = TRAIT::cos( q(RH_HAA));
+
+ (*this)(0,0) = - sin__q_RH_HFE__;
+ (*this)(0,1) = - cos__q_RH_HFE__;
+ (*this)(0,3) = ((- 0.35 * sin__q_RH_HFE__) - 0.3735);
+ (*this)(1,0) = ( sin__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(1,1) = (- sin__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(1,2) = cos__q_RH_HAA__;
+ (*this)(1,3) = (((( 0.35 * sin__q_RH_HAA__) * cos__q_RH_HFE__) + ( 0.08 * sin__q_RH_HAA__)) - 0.207);
+ (*this)(2,0) = (- cos__q_RH_HAA__ * cos__q_RH_HFE__);
+ (*this)(2,1) = ( cos__q_RH_HAA__ * sin__q_RH_HFE__);
+ (*this)(2,2) = sin__q_RH_HAA__;
+ (*this)(2,3) = (((- 0.35 * cos__q_RH_HAA__) * cos__q_RH_HFE__) - ( 0.08 * cos__q_RH_HAA__));
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LF_upperleg_X_fr_LF_hipassembly::Type_fr_LF_upperleg_X_fr_LF_hipassembly()
+{
+ (*this)(0,1) = 0;
+ (*this)(1,1) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = - 1;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LF_upperleg_X_fr_LF_hipassembly& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LF_upperleg_X_fr_LF_hipassembly::update(const JState& q) {
+ Scalar sin__q_LF_HFE__;
+ Scalar cos__q_LF_HFE__;
+
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+
+ (*this)(0,0) = cos__q_LF_HFE__;
+ (*this)(0,2) = sin__q_LF_HFE__;
+ (*this)(0,3) = (- 0.08 * cos__q_LF_HFE__);
+ (*this)(1,0) = - sin__q_LF_HFE__;
+ (*this)(1,2) = cos__q_LF_HFE__;
+ (*this)(1,3) = ( 0.08 * sin__q_LF_HFE__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LF_hipassembly_X_fr_LF_upperleg::Type_fr_LF_hipassembly_X_fr_LF_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0.08;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 0;
+ (*this)(1,2) = - 1;
+ (*this)(1,3) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LF_hipassembly_X_fr_LF_upperleg& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LF_hipassembly_X_fr_LF_upperleg::update(const JState& q) {
+ Scalar sin__q_LF_HFE__;
+ Scalar cos__q_LF_HFE__;
+
+ sin__q_LF_HFE__ = TRAIT::sin( q(LF_HFE));
+ cos__q_LF_HFE__ = TRAIT::cos( q(LF_HFE));
+
+ (*this)(0,0) = cos__q_LF_HFE__;
+ (*this)(0,1) = - sin__q_LF_HFE__;
+ (*this)(2,0) = sin__q_LF_HFE__;
+ (*this)(2,1) = cos__q_LF_HFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LF_lowerleg_X_fr_LF_upperleg::Type_fr_LF_lowerleg_X_fr_LF_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(1,2) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LF_lowerleg_X_fr_LF_upperleg& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LF_lowerleg_X_fr_LF_upperleg::update(const JState& q) {
+ Scalar sin__q_LF_KFE__;
+ Scalar cos__q_LF_KFE__;
+
+ sin__q_LF_KFE__ = TRAIT::sin( q(LF_KFE));
+ cos__q_LF_KFE__ = TRAIT::cos( q(LF_KFE));
+
+ (*this)(0,0) = cos__q_LF_KFE__;
+ (*this)(0,1) = sin__q_LF_KFE__;
+ (*this)(0,3) = (- 0.35 * cos__q_LF_KFE__);
+ (*this)(1,0) = - sin__q_LF_KFE__;
+ (*this)(1,1) = cos__q_LF_KFE__;
+ (*this)(1,3) = ( 0.35 * sin__q_LF_KFE__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LF_upperleg_X_fr_LF_lowerleg::Type_fr_LF_upperleg_X_fr_LF_lowerleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0.35;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LF_upperleg_X_fr_LF_lowerleg& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LF_upperleg_X_fr_LF_lowerleg::update(const JState& q) {
+ Scalar sin__q_LF_KFE__;
+ Scalar cos__q_LF_KFE__;
+
+ sin__q_LF_KFE__ = TRAIT::sin( q(LF_KFE));
+ cos__q_LF_KFE__ = TRAIT::cos( q(LF_KFE));
+
+ (*this)(0,0) = cos__q_LF_KFE__;
+ (*this)(0,1) = - sin__q_LF_KFE__;
+ (*this)(1,0) = sin__q_LF_KFE__;
+ (*this)(1,1) = cos__q_LF_KFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RF_upperleg_X_fr_RF_hipassembly::Type_fr_RF_upperleg_X_fr_RF_hipassembly()
+{
+ (*this)(0,1) = 0;
+ (*this)(1,1) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 1;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RF_upperleg_X_fr_RF_hipassembly& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RF_upperleg_X_fr_RF_hipassembly::update(const JState& q) {
+ Scalar sin__q_RF_HFE__;
+ Scalar cos__q_RF_HFE__;
+
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+
+ (*this)(0,0) = cos__q_RF_HFE__;
+ (*this)(0,2) = - sin__q_RF_HFE__;
+ (*this)(0,3) = (- 0.08 * cos__q_RF_HFE__);
+ (*this)(1,0) = - sin__q_RF_HFE__;
+ (*this)(1,2) = - cos__q_RF_HFE__;
+ (*this)(1,3) = ( 0.08 * sin__q_RF_HFE__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RF_hipassembly_X_fr_RF_upperleg::Type_fr_RF_hipassembly_X_fr_RF_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0.08;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 0;
+ (*this)(1,2) = 1;
+ (*this)(1,3) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RF_hipassembly_X_fr_RF_upperleg& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RF_hipassembly_X_fr_RF_upperleg::update(const JState& q) {
+ Scalar sin__q_RF_HFE__;
+ Scalar cos__q_RF_HFE__;
+
+ sin__q_RF_HFE__ = TRAIT::sin( q(RF_HFE));
+ cos__q_RF_HFE__ = TRAIT::cos( q(RF_HFE));
+
+ (*this)(0,0) = cos__q_RF_HFE__;
+ (*this)(0,1) = - sin__q_RF_HFE__;
+ (*this)(2,0) = - sin__q_RF_HFE__;
+ (*this)(2,1) = - cos__q_RF_HFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RF_lowerleg_X_fr_RF_upperleg::Type_fr_RF_lowerleg_X_fr_RF_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(1,2) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RF_lowerleg_X_fr_RF_upperleg& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RF_lowerleg_X_fr_RF_upperleg::update(const JState& q) {
+ Scalar sin__q_RF_KFE__;
+ Scalar cos__q_RF_KFE__;
+
+ sin__q_RF_KFE__ = TRAIT::sin( q(RF_KFE));
+ cos__q_RF_KFE__ = TRAIT::cos( q(RF_KFE));
+
+ (*this)(0,0) = cos__q_RF_KFE__;
+ (*this)(0,1) = sin__q_RF_KFE__;
+ (*this)(0,3) = (- 0.35 * cos__q_RF_KFE__);
+ (*this)(1,0) = - sin__q_RF_KFE__;
+ (*this)(1,1) = cos__q_RF_KFE__;
+ (*this)(1,3) = ( 0.35 * sin__q_RF_KFE__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RF_upperleg_X_fr_RF_lowerleg::Type_fr_RF_upperleg_X_fr_RF_lowerleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0.35;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RF_upperleg_X_fr_RF_lowerleg& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RF_upperleg_X_fr_RF_lowerleg::update(const JState& q) {
+ Scalar sin__q_RF_KFE__;
+ Scalar cos__q_RF_KFE__;
+
+ sin__q_RF_KFE__ = TRAIT::sin( q(RF_KFE));
+ cos__q_RF_KFE__ = TRAIT::cos( q(RF_KFE));
+
+ (*this)(0,0) = cos__q_RF_KFE__;
+ (*this)(0,1) = - sin__q_RF_KFE__;
+ (*this)(1,0) = sin__q_RF_KFE__;
+ (*this)(1,1) = cos__q_RF_KFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LH_upperleg_X_fr_LH_hipassembly::Type_fr_LH_upperleg_X_fr_LH_hipassembly()
+{
+ (*this)(0,1) = 0;
+ (*this)(1,1) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = - 1;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LH_upperleg_X_fr_LH_hipassembly& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LH_upperleg_X_fr_LH_hipassembly::update(const JState& q) {
+ Scalar sin__q_LH_HFE__;
+ Scalar cos__q_LH_HFE__;
+
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+
+ (*this)(0,0) = cos__q_LH_HFE__;
+ (*this)(0,2) = sin__q_LH_HFE__;
+ (*this)(0,3) = (- 0.08 * cos__q_LH_HFE__);
+ (*this)(1,0) = - sin__q_LH_HFE__;
+ (*this)(1,2) = cos__q_LH_HFE__;
+ (*this)(1,3) = ( 0.08 * sin__q_LH_HFE__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LH_hipassembly_X_fr_LH_upperleg::Type_fr_LH_hipassembly_X_fr_LH_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0.08;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 0;
+ (*this)(1,2) = - 1;
+ (*this)(1,3) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LH_hipassembly_X_fr_LH_upperleg& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LH_hipassembly_X_fr_LH_upperleg::update(const JState& q) {
+ Scalar sin__q_LH_HFE__;
+ Scalar cos__q_LH_HFE__;
+
+ sin__q_LH_HFE__ = TRAIT::sin( q(LH_HFE));
+ cos__q_LH_HFE__ = TRAIT::cos( q(LH_HFE));
+
+ (*this)(0,0) = cos__q_LH_HFE__;
+ (*this)(0,1) = - sin__q_LH_HFE__;
+ (*this)(2,0) = sin__q_LH_HFE__;
+ (*this)(2,1) = cos__q_LH_HFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LH_lowerleg_X_fr_LH_upperleg::Type_fr_LH_lowerleg_X_fr_LH_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(1,2) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LH_lowerleg_X_fr_LH_upperleg& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LH_lowerleg_X_fr_LH_upperleg::update(const JState& q) {
+ Scalar sin__q_LH_KFE__;
+ Scalar cos__q_LH_KFE__;
+
+ sin__q_LH_KFE__ = TRAIT::sin( q(LH_KFE));
+ cos__q_LH_KFE__ = TRAIT::cos( q(LH_KFE));
+
+ (*this)(0,0) = cos__q_LH_KFE__;
+ (*this)(0,1) = sin__q_LH_KFE__;
+ (*this)(0,3) = (- 0.35 * cos__q_LH_KFE__);
+ (*this)(1,0) = - sin__q_LH_KFE__;
+ (*this)(1,1) = cos__q_LH_KFE__;
+ (*this)(1,3) = ( 0.35 * sin__q_LH_KFE__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LH_upperleg_X_fr_LH_lowerleg::Type_fr_LH_upperleg_X_fr_LH_lowerleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0.35;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LH_upperleg_X_fr_LH_lowerleg& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_LH_upperleg_X_fr_LH_lowerleg::update(const JState& q) {
+ Scalar sin__q_LH_KFE__;
+ Scalar cos__q_LH_KFE__;
+
+ sin__q_LH_KFE__ = TRAIT::sin( q(LH_KFE));
+ cos__q_LH_KFE__ = TRAIT::cos( q(LH_KFE));
+
+ (*this)(0,0) = cos__q_LH_KFE__;
+ (*this)(0,1) = - sin__q_LH_KFE__;
+ (*this)(1,0) = sin__q_LH_KFE__;
+ (*this)(1,1) = cos__q_LH_KFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RH_upperleg_X_fr_RH_hipassembly::Type_fr_RH_upperleg_X_fr_RH_hipassembly()
+{
+ (*this)(0,1) = 0;
+ (*this)(1,1) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 1;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RH_upperleg_X_fr_RH_hipassembly& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RH_upperleg_X_fr_RH_hipassembly::update(const JState& q) {
+ Scalar sin__q_RH_HFE__;
+ Scalar cos__q_RH_HFE__;
+
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+
+ (*this)(0,0) = cos__q_RH_HFE__;
+ (*this)(0,2) = - sin__q_RH_HFE__;
+ (*this)(0,3) = (- 0.08 * cos__q_RH_HFE__);
+ (*this)(1,0) = - sin__q_RH_HFE__;
+ (*this)(1,2) = - cos__q_RH_HFE__;
+ (*this)(1,3) = ( 0.08 * sin__q_RH_HFE__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RH_hipassembly_X_fr_RH_upperleg::Type_fr_RH_hipassembly_X_fr_RH_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0.08;
+ (*this)(1,0) = 0;
+ (*this)(1,1) = 0;
+ (*this)(1,2) = 1;
+ (*this)(1,3) = 0;
+ (*this)(2,2) = 0;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RH_hipassembly_X_fr_RH_upperleg& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RH_hipassembly_X_fr_RH_upperleg::update(const JState& q) {
+ Scalar sin__q_RH_HFE__;
+ Scalar cos__q_RH_HFE__;
+
+ sin__q_RH_HFE__ = TRAIT::sin( q(RH_HFE));
+ cos__q_RH_HFE__ = TRAIT::cos( q(RH_HFE));
+
+ (*this)(0,0) = cos__q_RH_HFE__;
+ (*this)(0,1) = - sin__q_RH_HFE__;
+ (*this)(2,0) = - sin__q_RH_HFE__;
+ (*this)(2,1) = - cos__q_RH_HFE__;
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RH_lowerleg_X_fr_RH_upperleg::Type_fr_RH_lowerleg_X_fr_RH_upperleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(1,2) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1.0;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RH_lowerleg_X_fr_RH_upperleg& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RH_lowerleg_X_fr_RH_upperleg::update(const JState& q) {
+ Scalar sin__q_RH_KFE__;
+ Scalar cos__q_RH_KFE__;
+
+ sin__q_RH_KFE__ = TRAIT::sin( q(RH_KFE));
+ cos__q_RH_KFE__ = TRAIT::cos( q(RH_KFE));
+
+ (*this)(0,0) = cos__q_RH_KFE__;
+ (*this)(0,1) = sin__q_RH_KFE__;
+ (*this)(0,3) = (- 0.35 * cos__q_RH_KFE__);
+ (*this)(1,0) = - sin__q_RH_KFE__;
+ (*this)(1,1) = cos__q_RH_KFE__;
+ (*this)(1,3) = ( 0.35 * sin__q_RH_KFE__);
+ return *this;
+}
+template <typename TRAIT>
+iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RH_upperleg_X_fr_RH_lowerleg::Type_fr_RH_upperleg_X_fr_RH_lowerleg()
+{
+ (*this)(0,2) = 0;
+ (*this)(0,3) = 0.35;
+ (*this)(1,2) = 0;
+ (*this)(1,3) = 0;
+ (*this)(2,0) = 0;
+ (*this)(2,1) = 0;
+ (*this)(2,2) = 1;
+ (*this)(2,3) = 0;
+ (*this)(3,0) = 0;
+ (*this)(3,1) = 0;
+ (*this)(3,2) = 0;
+ (*this)(3,3) = 1;
+}
+template <typename TRAIT>
+const typename iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RH_upperleg_X_fr_RH_lowerleg& iit::TestHyQ::tpl::HomogeneousTransforms<TRAIT>::Type_fr_RH_upperleg_X_fr_RH_lowerleg::update(const JState& q) {
+ Scalar sin__q_RH_KFE__;
+ Scalar cos__q_RH_KFE__;
+
+ sin__q_RH_KFE__ = TRAIT::sin( q(RH_KFE));
+ cos__q_RH_KFE__ = TRAIT::cos( q(RH_KFE));
+
+ (*this)(0,0) = cos__q_RH_KFE__;
+ (*this)(0,1) = - sin__q_RH_KFE__;
+ (*this)(1,0) = sin__q_RH_KFE__;
+ (*this)(1,1) = cos__q_RH_KFE__;
+ return *this;
+}
+
diff --git a/ct_rbd/test/operationalSpace/OperationalSpaceTest.cpp b/ct_rbd/test/operationalSpace/OperationalSpaceTest.cpp
new file mode 100644
index 0000000..7a22dfc
--- /dev/null
+++ b/ct_rbd/test/operationalSpace/OperationalSpaceTest.cpp
@@ -0,0 +1,91 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <memory>
+#include <array>
+
+#include <Eigen/Dense>
+
+#include <gtest/gtest.h>
+
+#include <ct/optcon/optcon.h>
+
+#include <ct/rbd/robot/jacobian/OperationalJacobianBase.h>
+#include <ct/rbd/operationalSpace/rigid_body/OperationalModel.h>
+#include <ct/rbd/operationalSpace/rigid_body/OperationalModelRBD.h>
+#include "../models/testhyq/RobCoGenTestHyQ.h"
+
+using namespace ct::rbd;
+
+
+class TestJacobian : public OperationalJacobianBase<3, 12>
+{
+public:
+ typedef ct::rbd::OperationalJacobianBase<3, 12> Base;
+ typedef typename Base::state_t state_t;
+ typedef typename Base::jacobian_t origin_jacobian_t;
+
+ TestJacobian() {}
+ ~TestJacobian() {}
+ void getJacobianOrigin(const state_t& state, origin_jacobian_t& J) override { J.setIdentity(); }
+ void getJacobianOriginDerivative(const state_t& state, origin_jacobian_t& dJdt) override { dJdt.setZero(); }
+};
+
+
+TEST(OperationalSpaceTest, OperationalSpaceTest)
+{
+ size_t nTests = 100;
+
+ for (size_t i = 0; i < nTests; i++)
+ {
+ typedef Eigen::Matrix<double, 18, 1> robot_coordinate_t;
+ typedef Eigen::Matrix<double, 3, 1> operational_coordinate_t;
+ typedef typename OperationalModel<3, 12, 0>::state_t state_t;
+
+ // pointer to the test robot RBDContainer
+ std::shared_ptr<TestHyQ::RobCoGenContainer> testRbdContainerPtr(new TestHyQ::RobCoGenContainer());
+ // end-effector array for the possible contact points
+ std::array<EndEffector<12>, 0> EndEffectorArray;
+
+ // test robot model
+ OperationalModelRBD<TestHyQ::RobCoGenContainer, 0>::ptr testRobotModel(
+ new OperationalModelRBD<TestHyQ::RobCoGenContainer, 0>(testRbdContainerPtr, EndEffectorArray));
+ // test robot Jacobian
+ TestJacobian::ptr testJacobian(new TestJacobian());
+ // operational space
+ OperationalModel<3, 12, 0> testOperationalModel(testRobotModel, testJacobian);
+
+ Eigen::Matrix<double, 12, 1> tau = Eigen::Matrix<double, 12, 1>::Random();
+ state_t state;
+ state.fromStateVectorEulerXyz(state_t::state_vector_euler_t::Random());
+ //robot_coordinate_t q = state.toCoordinatePosition();
+ //robot_coordinate_t qd = state.toCoordinateVelocity();
+
+ // update the robot model
+ testRobotModel->update(state);
+ robot_coordinate_t qdd;
+ qdd = testRobotModel->MInverse() *
+ (-testRobotModel->C() - testRobotModel->G() + testRobotModel->S().transpose() * tau);
+
+ // update the operational space model
+ testOperationalModel.update(state);
+ operational_coordinate_t xdd = testOperationalModel.getAccelerations(qdd);
+
+ operational_coordinate_t testModelError = testOperationalModel.M() * xdd + testOperationalModel.C() +
+ testOperationalModel.G() - testOperationalModel.S().transpose() * tau;
+
+ double maxError = testModelError.cwiseAbs().maxCoeff();
+
+ ASSERT_LT(maxError, 1e-4);
+ }
+}
+
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_rbd/test/physics/EEContactModelTest.cpp b/ct_rbd/test/physics/EEContactModelTest.cpp
new file mode 100644
index 0000000..f288387
--- /dev/null
+++ b/ct_rbd/test/physics/EEContactModelTest.cpp
@@ -0,0 +1,51 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <ct/rbd/rbd.h>
+
+#include <memory>
+#include <gtest/gtest.h>
+
+#include <ct/rbd/physics/EEContactModel.h>
+
+#include "../models/testhyq/RobCoGenTestHyQ.h"
+
+using namespace ct::rbd;
+
+
+TEST(EEContactModelTest, basicTest)
+{
+ typedef TestHyQ::Kinematics HyqKinematics;
+ typedef typename EEContactModel<HyqKinematics>::EEForcesLinear EEForcesLinear;
+
+ EEContactModel<HyqKinematics> eeContactModel;
+
+ RBDState<HyqKinematics::NJOINTS> state;
+ state.setRandom();
+
+ EEForcesLinear forces;
+
+ try
+ {
+ forces = eeContactModel.computeContactForces(state);
+ } catch (const std::runtime_error& e)
+ {
+ std::cout << "error thrown: " << e.what() << std::endl;
+ ASSERT_TRUE(false);
+ }
+
+ for (size_t i = 0; i < forces.size(); i++)
+ {
+ std::cout << "Force at EE-ID " << i << ": " << forces[i].transpose();
+ }
+}
+
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_rbd/test/robot/control/ControllerTests.cpp b/ct_rbd/test/robot/control/ControllerTests.cpp
new file mode 100644
index 0000000..929e6a2
--- /dev/null
+++ b/ct_rbd/test/robot/control/ControllerTests.cpp
@@ -0,0 +1,37 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <gtest/gtest.h>
+
+#include <ct/core/core.h>
+#include <ct/rbd/rbd.h>
+
+#include <ct/rbd/systems/linear/RbdLinearizer.h>
+
+#include <models/testIrb4600/RobCoGenTestIrb4600.h>
+
+
+TEST(RbdControllerTests, RbdControllerTests)
+{
+ /*
+ const size_t state_dim = ct::rbd::FixBaseFDSystem<ct::rbd::TestIrb4600::Dynamics>::STATE_DIM;
+ const size_t control_dim = ct::rbd::FixBaseFDSystem<ct::rbd::TestIrb4600::Dynamics>::CONTROL_DIM;
+
+ std::shared_ptr<ct::rbd::FixBaseFDSystem<ct::rbd::TestIrb4600::Dynamics>> robot (new ct::rbd::FixBaseFDSystem<ct::rbd::TestIrb4600::Dynamics>);
+
+ ct::core::SystemLinearizer<state_dim, control_dim> systemLinearizer(robot);
+
+ todo: unit test ausbauen, gestoppt da irb system noch nicht korrekt.
+
+*/
+}
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_rbd/test/robot/costfunction/TaskspaceCostFunctionTest.cpp b/ct_rbd/test/robot/costfunction/TaskspaceCostFunctionTest.cpp
new file mode 100644
index 0000000..48d380b
--- /dev/null
+++ b/ct_rbd/test/robot/costfunction/TaskspaceCostFunctionTest.cpp
@@ -0,0 +1,17 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <ct/rbd/rbd.h>
+#include "TaskspaceCostFunctionTest.h"
+
+using namespace ct;
+using namespace rbd;
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_rbd/test/robot/costfunction/TaskspaceCostFunctionTest.h b/ct_rbd/test/robot/costfunction/TaskspaceCostFunctionTest.h
new file mode 100644
index 0000000..564d640
--- /dev/null
+++ b/ct_rbd/test/robot/costfunction/TaskspaceCostFunctionTest.h
@@ -0,0 +1,135 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include "../../models/testhyq/RobCoGenTestHyQ.h"
+#include <gtest/gtest.h>
+#include "../../../ct_optcon/test/costfunction/compareCostFunctions.h"
+
+using namespace ct;
+using namespace rbd;
+
+TEST(TaskspaceCostFunctionTests, TestTaskSpacePositionTerm)
+{
+ typedef ct::core::ADCGScalar size_type;
+ typedef TestHyQ::tpl::Kinematics<size_type> KinTpl_t;
+
+ KinTpl_t kynTpl;
+ size_t eeId = 1;
+
+ const size_t hyqStateDim = 36;
+ const size_t hyqControlDim = 12;
+
+ Eigen::Matrix<double, 3, 3> Q;
+ Q.setIdentity();
+
+ std::shared_ptr<optcon::CostFunctionAD<hyqStateDim, hyqControlDim>> Adcf(
+ new optcon::CostFunctionAD<hyqStateDim, hyqControlDim>());
+ std::shared_ptr<TermTaskspacePosition<KinTpl_t, true, hyqStateDim, hyqControlDim>> term1(
+ new TermTaskspacePosition<KinTpl_t, true, hyqStateDim, hyqControlDim>(eeId, Q, Eigen::Vector3d::Random()));
+
+ Adcf->addFinalADTerm(term1, true);
+ Adcf->addIntermediateADTerm(term1, true);
+
+ Adcf->initialize();
+
+ Eigen::Matrix<double, hyqStateDim, 1> x;
+ Eigen::Matrix<double, hyqControlDim, 1> u;
+ x.setRandom();
+ u.setRandom();
+
+ double t = 1.0;
+
+ Adcf->setCurrentStateAndControl(x, u, t);
+
+ // Adcf->stateDerivativeIntermediateTest();
+ // Adcf->controlDerivativeIntermediateTest();
+
+ //! compare auto-diff against num-diff
+ ASSERT_TRUE(Adcf->stateDerivativeIntermediateTest());
+ ASSERT_TRUE(Adcf->controlDerivativeIntermediateTest());
+
+ // check cloning for this term/costfunction combination
+ std::shared_ptr<optcon::CostFunctionAD<hyqStateDim, hyqControlDim>> Adcf_cloned(Adcf->clone());
+ x.setRandom();
+ u.setRandom();
+ t = 2.0;
+ Adcf->setCurrentStateAndControl(x, u, t);
+ Adcf_cloned->setCurrentStateAndControl(x, u, t);
+
+ ct::optcon::example::compareCostFunctionOutput(*Adcf_cloned, *Adcf);
+}
+
+
+TEST(TaskspaceCostFunctionTests, TestTaskSpacePoseTerm)
+{
+ typedef ct::core::ADCGScalar size_type;
+ typedef TestHyQ::tpl::Kinematics<size_type> KinTpl_t;
+
+ KinTpl_t kynTpl;
+ size_t eeId = 1;
+
+ const size_t hyqStateDim = 36;
+ const size_t hyqControlDim = 12;
+
+ // specify a penalty on the position error
+ Eigen::Matrix<double, 3, 3> Qpos;
+ Qpos.setIdentity();
+
+ // specify a penalty on the orientation error
+ double Qrot = 1.0;
+
+ // specify a desired ee position
+ Eigen::Matrix<double, 3, 1> w_pos_ee;
+ w_pos_ee.setRandom();
+
+ // specify a desired ee orientation as quaternion, to be used in first constructor
+ Eigen::Quaternion<double> w_q_ee;
+ w_q_ee.setIdentity();
+
+ // get euler angles corresponding to the quaternion, to be used in second constructor
+ Eigen::Matrix<double, 3, 1> w_euler_ee = w_q_ee.toRotationMatrix().eulerAngles(0, 1, 2);
+
+ std::shared_ptr<optcon::CostFunctionAD<hyqStateDim, hyqControlDim>> Adcf(
+ new optcon::CostFunctionAD<hyqStateDim, hyqControlDim>());
+
+ // test constructor taking the quaternion
+ std::shared_ptr<TermTaskspacePose<KinTpl_t, true, hyqStateDim, hyqControlDim>> term1(
+ new TermTaskspacePose<KinTpl_t, true, hyqStateDim, hyqControlDim>(eeId, Qpos, Qrot, w_pos_ee, w_q_ee));
+
+ // test constructor using euler angles
+ std::shared_ptr<TermTaskspacePose<KinTpl_t, true, hyqStateDim, hyqControlDim>> term2(
+ new TermTaskspacePose<KinTpl_t, true, hyqStateDim, hyqControlDim>(eeId, Qpos, Qrot, w_pos_ee, w_euler_ee));
+
+ Adcf->addFinalADTerm(term1, true);
+ Adcf->addIntermediateADTerm(term1, true);
+
+ Adcf->initialize();
+
+ Eigen::Matrix<double, hyqStateDim, 1> x;
+ Eigen::Matrix<double, hyqControlDim, 1> u;
+ x.setRandom();
+ u.setRandom();
+
+ double t = 1.0;
+
+ Adcf->setCurrentStateAndControl(x, u, t);
+
+ // Adcf->stateDerivativeIntermediateTest();
+ // Adcf->controlDerivativeIntermediateTest();
+
+ //! compare auto-diff against num-diff
+ ASSERT_TRUE(Adcf->stateDerivativeIntermediateTest());
+ ASSERT_TRUE(Adcf->controlDerivativeIntermediateTest());
+
+ // check cloning for this term/costfunction combination
+ std::shared_ptr<optcon::CostFunctionAD<hyqStateDim, hyqControlDim>> Adcf_cloned(Adcf->clone());
+ x.setRandom();
+ u.setRandom();
+ t = 2.0;
+ Adcf->setCurrentStateAndControl(x, u, t);
+ Adcf_cloned->setCurrentStateAndControl(x, u, t);
+ ct::optcon::example::compareCostFunctionOutput(*Adcf_cloned, *Adcf);
+}
diff --git a/ct_rbd/test/robot/costfunction/kindrJITtest.cpp b/ct_rbd/test/robot/costfunction/kindrJITtest.cpp
new file mode 100644
index 0000000..100b581
--- /dev/null
+++ b/ct_rbd/test/robot/costfunction/kindrJITtest.cpp
@@ -0,0 +1,20 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <gtest/gtest.h>
+#include <ct/core/core.h>
+
+
+using namespace ct::core;
+using std::shared_ptr;
+
+#include "kindrJITtest.h"
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_rbd/test/robot/costfunction/kindrJITtest.h b/ct_rbd/test/robot/costfunction/kindrJITtest.h
new file mode 100644
index 0000000..71f48ec
--- /dev/null
+++ b/ct_rbd/test/robot/costfunction/kindrJITtest.h
@@ -0,0 +1,693 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <kindr/Core>
+
+
+/*!
+ * A 3x3 function which constructs a a kindr EulerAngles object in between.
+ * The aim of this function is to verify that JIT gets along with kindr Euler angle types
+ * (which it should do naturally)
+ *
+ * @param x the input
+ * @return output y = f(x)
+ */
+template <typename SCALAR>
+Eigen::Matrix<SCALAR, 3, 1> testFunction(const Eigen::Matrix<SCALAR, 3, 1>& x)
+{
+ kindr::EulerAnglesXyz<SCALAR> anglesIn(x);
+ kindr::EulerAnglesXyz<SCALAR> anglesTemp;
+
+ anglesTemp.setX(3 * anglesIn.x() + 2 * anglesIn.x() * anglesIn.x() - anglesIn.y() * anglesIn.z());
+ anglesTemp.setY(anglesIn.z() + anglesIn.y() + 3);
+ anglesTemp.setZ(anglesIn.z());
+
+ kindr::EulerAnglesXyz<SCALAR> anglesOut(anglesTemp);
+
+ return anglesOut.toImplementation();
+}
+
+/*!
+ * The analytically, manually derived Jacobian of testFunction() used for verification
+ *
+ * @param x the input
+ * @return output y = f(x)
+ */
+template <typename SCALAR>
+Eigen::Matrix<SCALAR, 3, 3> jacobianCheck(const Eigen::Matrix<SCALAR, 3, 1>& x)
+{
+ Eigen::Matrix<SCALAR, 3, 3> jac;
+
+ jac << 3 + 4 * x(0), -x(2), -x(1), 0, 1, 1, 0, 0, 1;
+
+ return jac;
+}
+
+
+/*!
+ * Test for just-in-time compilation of the Jacobian and subsequent evaluation of it, with kindr in the loop
+ */
+TEST(KindrJitTest, EulerAnglesTest)
+{
+ using derivativesCppadJIT = ct::core::DerivativesCppadJIT<3, 3>;
+ try
+ {
+ // create a function handle (also works for class methods, lambdas, function pointers, ...)
+ typename derivativesCppadJIT::FUN_TYPE_CG f = testFunction<derivativesCppadJIT::CG_SCALAR>;
+
+ // initialize the Auto-Diff Codegen Jacobian
+ derivativesCppadJIT jacCG(f);
+
+ DerivativesCppadSettings settings;
+ settings.createJacobian_ = true;
+
+ // compile the Jacobian
+ jacCG.compileJIT(settings, "eulerAnglesTestLib");
+
+ // create an input vector
+ Eigen::Matrix<double, 3, 1> angles;
+
+ for (size_t i = 0; i < 10; i++)
+ {
+ // create a random input
+ angles.setRandom();
+
+ // verify agains the analytical Jacobian
+
+ ASSERT_LT((jacCG.jacobian(angles) - jacobianCheck(angles)).array().abs().maxCoeff(), 1e-10);
+ }
+ } catch (std::exception& e)
+ {
+ std::cout << "Kindr Euler angle test failed: " << e.what() << std::endl;
+ ASSERT_TRUE(false);
+ }
+}
+
+
+//! test the kindr rotate method on euler angles
+template <typename SCALAR>
+Eigen::Matrix<SCALAR, 3, 1> rotateTestFunction(const Eigen::Matrix<SCALAR, 3, 1>& x)
+{
+ kindr::EulerAnglesXyz<SCALAR> angles(x);
+
+ // create a position and set to a trivial hard-coded value
+ kindr::Position<SCALAR, 3> pos;
+ pos.toImplementation()(0) = 1;
+ pos.toImplementation()(1) = 1;
+ pos.toImplementation()(2) = 1;
+
+ return angles.rotate(pos).toImplementation();
+}
+TEST(KindrJitTest, EulerRotateTest)
+{
+ using derivativesCppadJIT = ct::core::DerivativesCppadJIT<3, 3>;
+ try
+ {
+ // create a function handle (also works for class methods, lambdas, function pointers, ...)
+ typename derivativesCppadJIT::FUN_TYPE_CG f = rotateTestFunction<derivativesCppadJIT::CG_SCALAR>;
+
+ // initialize the Auto-Diff Codegen Jacobian
+ derivativesCppadJIT jacCG(f);
+
+ DerivativesCppadSettings settings;
+ settings.createJacobian_ = true;
+
+ // compile the Jacobian
+ jacCG.compileJIT(settings, "EulerRotateTest");
+
+ } catch (std::exception& e)
+ {
+ std::cout << "Kindr rotate test failed: " << e.what() << std::endl;
+ ASSERT_TRUE(false);
+ }
+}
+
+
+//! test the kindr RotationMatrix constructor
+template <typename SCALAR>
+Eigen::Matrix<SCALAR, 3, 1> kindrRotationMatrixTestFunction(const Eigen::Matrix<SCALAR, 3, 1>& x)
+{
+ kindr::EulerAnglesXyz<SCALAR> angles(x);
+
+ // create a position and set to a trivial hard-coded value
+ Eigen::Matrix<SCALAR, 3, 1> pos;
+ pos(0) = 1;
+ pos(1) = 1;
+ pos(2) = 1;
+
+ Eigen::Matrix<SCALAR, 3, 3> someMatrix;
+ someMatrix.setIdentity();
+ someMatrix(0, 0) = ct::core::internal::CppADCodegenTrait::cos(x(0));
+ someMatrix(1, 1) = ct::core::internal::CppADCodegenTrait::cos(x(0));
+ someMatrix(0, 1) = -ct::core::internal::CppADCodegenTrait::sin(x(0));
+ someMatrix(1, 0) = ct::core::internal::CppADCodegenTrait::sin(x(0));
+
+ kindr::RotationMatrix<SCALAR> rotMat(someMatrix);
+
+ return rotMat.toImplementation() * pos;
+}
+TEST(KindrJitTest, RotationMatrixTest)
+{
+ using derivativesCppadJIT = ct::core::DerivativesCppadJIT<3, 3>;
+
+ try
+ {
+ // create a function handle (also works for class methods, lambdas, function pointers, ...)
+ typename derivativesCppadJIT::FUN_TYPE_CG f = kindrRotationMatrixTestFunction<derivativesCppadJIT::CG_SCALAR>;
+
+ // initialize the Auto-Diff Codegen Jacobian
+ derivativesCppadJIT jacCG(f);
+
+ DerivativesCppadSettings settings;
+ settings.createJacobian_ = true;
+ settings.createForwardZero_ = true;
+
+ // compile the Jacobian
+ jacCG.compileJIT(settings, "rotationMatrixTestLib");
+
+ } catch (std::exception& e)
+ {
+ std::cout << "Kindr rotation matrix test failed: " << e.what() << std::endl;
+ ASSERT_TRUE(false);
+ }
+}
+
+
+//! test the behaviour of transcribing a kindr rotation matrix into Euler Angles
+template <typename SCALAR>
+Eigen::Matrix<SCALAR, 3, 1> convertRotationMatrixToKindrEulerTestFunction(const Eigen::Matrix<SCALAR, 3, 1>& x)
+{
+ Eigen::Matrix<SCALAR, 3, 3> someMatrix;
+ someMatrix.setIdentity();
+ someMatrix(0, 0) = ct::core::internal::CppADCodegenTrait::cos(x(0));
+ someMatrix(1, 1) = ct::core::internal::CppADCodegenTrait::cos(x(0));
+ someMatrix(0, 1) = -ct::core::internal::CppADCodegenTrait::sin(x(0));
+ someMatrix(1, 0) = ct::core::internal::CppADCodegenTrait::sin(x(0));
+
+ kindr::RotationMatrix<SCALAR> rotMat(someMatrix);
+
+ kindr::EulerAnglesXyz<SCALAR> euler(rotMat); //<-- fails
+
+ return euler.toImplementation();
+}
+TEST(KindrJitTest, DISABLED_KindrRotationMatrixToEulerAnglesXyzTest)
+{
+ using derivativesCppadJIT = ct::core::DerivativesCppadJIT<3, 3>;
+
+ try
+ {
+ // create a function handle (also works for class methods, lambdas, function pointers, ...)
+ typename derivativesCppadJIT::FUN_TYPE_CG f =
+ convertRotationMatrixToKindrEulerTestFunction<derivativesCppadJIT::CG_SCALAR>;
+
+ // initialize the Auto-Diff Codegen Jacobian
+ derivativesCppadJIT jacCG(f);
+
+ DerivativesCppadSettings settings;
+ settings.createJacobian_ = true;
+ settings.createForwardZero_ = true;
+
+ // compile the Jacobian
+ jacCG.compileJIT(settings, "rotationMatrixToEulerTestLib");
+
+ } catch (std::exception& e)
+ {
+ std::cout << "KindrRotationMatrixToEulerXyzTest test failed: " << e.what() << std::endl;
+ ASSERT_TRUE(false);
+ }
+}
+
+
+//! test the behaviour of transcribing an Eigen rotation matrix into Eigen Euler Angles
+template <typename SCALAR>
+Eigen::Matrix<SCALAR, 3, 1> convertRotationMatrixToEigenEulerTestFunction(const Eigen::Matrix<SCALAR, -1, 1>& x)
+{
+ Eigen::Matrix<SCALAR, 3, 3> someMatrix(3, 3);
+ someMatrix.setIdentity();
+ someMatrix(0, 0) = ct::core::internal::CppADCodegenTrait::cos(x(0));
+ someMatrix(1, 1) = ct::core::internal::CppADCodegenTrait::cos(x(0));
+ someMatrix(0, 1) = -ct::core::internal::CppADCodegenTrait::sin(x(0));
+ someMatrix(1, 0) = ct::core::internal::CppADCodegenTrait::sin(x(0));
+
+ Eigen::Matrix<SCALAR, 3, 1> eigenEulerAngles = someMatrix.eulerAngles(0, 1, 2); // <-- fails
+
+ return eigenEulerAngles;
+}
+TEST(KindrJitTest, DISABLED_EigenRotationMatrixToEigenEulerAnglesTest)
+{
+ using derivativesCppadJIT = ct::core::DerivativesCppadJIT<3, 3>;
+
+ try
+ {
+ // create a function handle (also works for class methods, lambdas, function pointers, ...)
+ typename derivativesCppadJIT::FUN_TYPE_CG f =
+ convertRotationMatrixToEigenEulerTestFunction<derivativesCppadJIT::CG_SCALAR>;
+
+ // initialize the Auto-Diff Codegen Jacobian
+ derivativesCppadJIT jacCG(f);
+
+ DerivativesCppadSettings settings;
+ settings.createJacobian_ = true;
+ settings.createForwardZero_ = true;
+
+ // compile the Jacobian
+ // Eigen::VectorXd test (3); test << 0.0, 0.0, 0.0;
+ // jacCG.forwardZero(test); // <--fails
+ jacCG.compileJIT(settings, "EigenRotationMatrixToEigenEulerAnglesTest");
+
+ } catch (std::exception& e)
+ {
+ std::cout << "EigenRotationMatrixToEigenEulerAnglesTest test failed: " << e.what() << std::endl;
+ ASSERT_TRUE(false);
+ }
+}
+
+
+#if EIGEN_VERSION_AT_LEAST(3, 2, 7) // prior versions have a bug
+//! test the behaviour of transcribing an Eigen rotation matrix into Eigen AngleAxis representation
+template <typename SCALAR>
+Eigen::Matrix<SCALAR, 3, 1> convertRotationMatrixToAngleAxisTestFunction(const Eigen::Matrix<SCALAR, -1, 1>& x)
+{
+ Eigen::Matrix<SCALAR, 3, 3> someMatrix(3, 3);
+ someMatrix.setIdentity();
+ someMatrix(0, 0) = ct::core::internal::CppADCodegenTrait::cos(x(0));
+ someMatrix(1, 1) = ct::core::internal::CppADCodegenTrait::cos(x(0));
+ someMatrix(0, 1) = -ct::core::internal::CppADCodegenTrait::sin(x(0));
+ someMatrix(1, 0) = ct::core::internal::CppADCodegenTrait::sin(x(0));
+
+ Eigen::AngleAxis<SCALAR> angleAxis(someMatrix);
+
+ return angleAxis.axis();
+}
+
+TEST(KindrJitTest, DISABLED_EigenRotationMatrixToAngleAxisTest)
+{
+ using derivativesCppadJIT = ct::core::DerivativesCppadJIT<3, 3>;
+
+ try
+ {
+ // create a function handle (also works for class methods, lambdas, function pointers, ...)
+ typename derivativesCppadJIT::FUN_TYPE_CG f =
+ convertRotationMatrixToAngleAxisTestFunction<derivativesCppadJIT::CG_SCALAR>;
+
+ // initialize the Auto-Diff Codegen Jacobian
+ derivativesCppadJIT jacCG(f);
+
+ DerivativesCppadSettings settings;
+ settings.createJacobian_ = true;
+
+ // compile the Jacobian
+ jacCG.compileJIT(settings, "EigenRotationMatrixToAngleAxisTest");
+
+ } catch (std::exception& e)
+ {
+ std::cout << "EigenRotationMatrixToAngleAxisTest test failed: " << e.what() << std::endl;
+ ASSERT_TRUE(false);
+ }
+}
+#endif
+
+
+//! test the Frobenius norm on an EigenMatrix
+template <typename SCALAR>
+Eigen::Matrix<SCALAR, -1, 1> frobeniusNormTestFunction(const Eigen::Matrix<SCALAR, -1, 1>& x)
+{
+ Eigen::Matrix<SCALAR, 3, 3> someMatrix(3, 3);
+ someMatrix.setIdentity();
+ someMatrix(0, 0) = ct::core::internal::CppADCodegenTrait::cos(x(0));
+ someMatrix(1, 1) = ct::core::internal::CppADCodegenTrait::cos(x(0));
+ someMatrix(0, 1) = -ct::core::internal::CppADCodegenTrait::sin(x(0));
+ someMatrix(1, 0) = ct::core::internal::CppADCodegenTrait::sin(x(0));
+
+ Eigen::Matrix<SCALAR, 1, 1> someData;
+ someData(0, 0) = someMatrix.norm(); // per default Eigen returns the Frobenius when calling norm() on a matrix.
+ return someData;
+}
+TEST(KindrJitTest, FrobeniusNormTest)
+{
+ using derivativesCppadJIT = ct::core::DerivativesCppadJIT<3, 1>;
+
+ try
+ {
+ // create a function handle (also works for class methods, lambdas, function pointers, ...)
+ typename derivativesCppadJIT::FUN_TYPE_CG f = frobeniusNormTestFunction<derivativesCppadJIT::CG_SCALAR>;
+
+ // initialize the Auto-Diff Codegen Jacobian
+ derivativesCppadJIT jacCG(f);
+
+ DerivativesCppadSettings settings;
+ settings.createJacobian_ = true;
+
+ // compile the Jacobian
+ jacCG.compileJIT(settings, "FrobeniusNormTest");
+
+ } catch (std::exception& e)
+ {
+ std::cout << "FrobeniusNormTest test failed: " << e.what() << std::endl;
+ ASSERT_TRUE(false);
+ }
+}
+
+
+//! test the behaviour of transcribing a rotation matrix into a kindr RotationQuaternion
+template <typename SCALAR>
+Eigen::Matrix<SCALAR, 4, 1> convertRotationMatrixToKindrQuatTestFunction(const Eigen::Matrix<SCALAR, 3, 1>& x)
+{
+ Eigen::Matrix<SCALAR, 3, 3> someMatrix;
+ someMatrix.setIdentity();
+ someMatrix(0, 0) = ct::core::internal::CppADCodegenTrait::cos(x(0));
+ someMatrix(1, 1) = ct::core::internal::CppADCodegenTrait::cos(x(0));
+ someMatrix(0, 1) = -ct::core::internal::CppADCodegenTrait::sin(x(0));
+ someMatrix(1, 0) = ct::core::internal::CppADCodegenTrait::sin(x(0));
+
+ // this works
+ kindr::RotationMatrix<SCALAR> rotMat(someMatrix);
+
+ // this does not, although it should.
+ kindr::RotationQuaternion<SCALAR> quat(rotMat); // <-- todo this fails
+
+ Eigen::Matrix<SCALAR, 4, 1> someData;
+ someData << quat.w(), quat.x(), quat.y(), quat.z();
+
+ return someData;
+}
+TEST(KindrJitTest, DISABLED_RotationMatrixToKindrRotationQuatTest)
+{
+ using derivativesCppadJIT = ct::core::DerivativesCppadJIT<3, 4>;
+
+ try
+ {
+ // create a function handle (also works for class methods, lambdas, function pointers, ...)
+ typename derivativesCppadJIT::FUN_TYPE_CG f =
+ convertRotationMatrixToKindrQuatTestFunction<derivativesCppadJIT::CG_SCALAR>;
+
+ // initialize the Auto-Diff Codegen Jacobian
+ derivativesCppadJIT jacCG(f);
+
+ DerivativesCppadSettings settings;
+ settings.createJacobian_ = true;
+ settings.createForwardZero_ = true;
+
+ // compile the Jacobian
+ jacCG.compileJIT(settings, "convertRotationMatrixToQuatTestFunction");
+
+ } catch (std::exception& e)
+ {
+ std::cout << "convertRotationMatrixToQuatTestFunction test failed: " << e.what() << std::endl;
+ ASSERT_TRUE(false);
+ }
+}
+
+
+//! check conversion from kindr rotation matrix to an eigen quaternion
+template <typename SCALAR>
+Eigen::Matrix<SCALAR, 4, 1> convertKindrRotationMatrixToEigenQuatTestFunction(const Eigen::Matrix<SCALAR, 3, 1>& x)
+{
+ Eigen::Matrix<SCALAR, 3, 3> someMatrix;
+ someMatrix.setIdentity();
+ someMatrix(0, 0) = ct::core::internal::CppADCodegenTrait::cos(x(0));
+ someMatrix(1, 1) = ct::core::internal::CppADCodegenTrait::cos(x(0));
+ someMatrix(0, 1) = -ct::core::internal::CppADCodegenTrait::sin(x(0));
+ someMatrix(1, 0) = ct::core::internal::CppADCodegenTrait::sin(x(0));
+
+ kindr::RotationMatrix<SCALAR> rotMat(someMatrix);
+
+ // this does not, although it should.
+ Eigen::Quaternion<SCALAR> quat(someMatrix); // <-- todo this fails
+
+ Eigen::Matrix<SCALAR, 4, 1> someData;
+ someData << quat.w(), quat.x(), quat.y(), quat.z();
+
+ return someData;
+}
+TEST(KindrJitTest, DISABLED_KindrRotationMatrixToEigenRotationQuatTest)
+{
+ using derivativesCppadJIT = ct::core::DerivativesCppadJIT<3, 4>;
+
+ try
+ {
+ // create a function handle (also works for class methods, lambdas, function pointers, ...)
+ typename derivativesCppadJIT::FUN_TYPE_CG f =
+ convertKindrRotationMatrixToEigenQuatTestFunction<derivativesCppadJIT::CG_SCALAR>;
+
+ // initialize the Auto-Diff Codegen Jacobian
+ derivativesCppadJIT jacCG(f);
+
+ DerivativesCppadSettings settings;
+ settings.createJacobian_ = true;
+ settings.createForwardZero_ = true;
+
+ // compile the Jacobian
+ jacCG.compileJIT(settings, "RotationMatrixToEigenRotationQuatTest");
+
+ } catch (std::exception& e)
+ {
+ std::cout << "RotationMatrixToEigenQuatTest test failed: " << e.what() << std::endl;
+ ASSERT_TRUE(false);
+ }
+}
+
+
+//! test the JIT compatibility of Eigen Transforms
+template <typename SCALAR>
+Eigen::Matrix<SCALAR, 3, 1> eigenTransformTestFunction(const Eigen::Matrix<SCALAR, 3, 1>& x)
+{
+ Eigen::Transform<SCALAR, 3, Eigen::Affine> t;
+ t = Eigen::Translation<SCALAR, 3>(x);
+ t.rotate(Eigen::AngleAxis<SCALAR>(x(0), Eigen::Matrix<SCALAR, 3, 1>::UnitX()));
+ t.rotate(Eigen::AngleAxis<SCALAR>(x(1), Eigen::Matrix<SCALAR, 3, 1>::UnitY()));
+ t.rotate(Eigen::AngleAxis<SCALAR>(x(2), Eigen::Matrix<SCALAR, 3, 1>::UnitZ()));
+
+ // return some random part of the eigen transform matrix
+ return (t.matrix()).template topLeftCorner<3, 1>();
+}
+TEST(KindrJitTest, EigenTransformTest)
+{
+ using derivativesCppadJIT = ct::core::DerivativesCppadJIT<3, 3>;
+
+ try
+ {
+ // create a function handle (also works for class methods, lambdas, function pointers, ...)
+ typename derivativesCppadJIT::FUN_TYPE_CG f = eigenTransformTestFunction<derivativesCppadJIT::CG_SCALAR>;
+
+ // initialize the Auto-Diff Codegen Jacobian
+ derivativesCppadJIT jacCG(f);
+
+ DerivativesCppadSettings settings;
+ settings.createJacobian_ = true;
+
+ // compile the Jacobian
+ jacCG.compileJIT(settings, "eigenTransformTestLib");
+
+ } catch (std::exception& e)
+ {
+ std::cout << "Eigen transform test failed: " << e.what() << std::endl;
+ ASSERT_TRUE(false);
+ }
+}
+
+
+//! test the JIT compatibility of Eigen quaternions
+template <typename SCALAR>
+Eigen::Matrix<SCALAR, 3, 1> eigenQuaternionTestFunction(const Eigen::Matrix<SCALAR, 3, 1>& x)
+{
+ Eigen::Quaternion<SCALAR> q;
+ q = Eigen::AngleAxis<SCALAR>(x(0), Eigen::Matrix<SCALAR, 3, 1>::UnitX()) *
+ Eigen::AngleAxis<SCALAR>(x(1), Eigen::Matrix<SCALAR, 3, 1>::UnitY()) *
+ Eigen::AngleAxis<SCALAR>(x(2), Eigen::Matrix<SCALAR, 3, 1>::UnitZ());
+
+ // return some data
+ return q.toRotationMatrix().template topRightCorner<3, 1>();
+}
+TEST(KindrJitTest, EigenQuaternionTest)
+{
+ using derivativesCppadJIT = ct::core::DerivativesCppadJIT<3, 3>;
+
+ try
+ {
+ // create a function handle (also works for class methods, lambdas, function pointers, ...)
+ typename derivativesCppadJIT::FUN_TYPE_CG f = eigenQuaternionTestFunction<derivativesCppadJIT::CG_SCALAR>;
+
+ // initialize the Auto-Diff Codegen Jacobian
+ derivativesCppadJIT jacCG(f);
+
+ DerivativesCppadSettings settings;
+ settings.createJacobian_ = true;
+
+ // compile the Jacobian
+ jacCG.compileJIT(settings, "EigenQuaternionTest");
+
+ } catch (std::exception& e)
+ {
+ std::cout << "Eigen transform test failed: " << e.what() << std::endl;
+ ASSERT_TRUE(false);
+ }
+}
+
+
+//! check the conversion between an Eigen Quaternion and an kindr RotationQuaternion.
+template <typename SCALAR>
+Eigen::Matrix<SCALAR, 4, 1> eigenQuatToKindrRotationQuatFunction(const Eigen::Matrix<SCALAR, 3, 1>& x)
+{
+ Eigen::Quaternion<SCALAR> q;
+ q = Eigen::AngleAxis<SCALAR>(x(0), Eigen::Matrix<SCALAR, 3, 1>::UnitX()) *
+ Eigen::AngleAxis<SCALAR>(x(1), Eigen::Matrix<SCALAR, 3, 1>::UnitY()) *
+ Eigen::AngleAxis<SCALAR>(x(2), Eigen::Matrix<SCALAR, 3, 1>::UnitZ());
+
+ kindr::RotationQuaternion<SCALAR> kindr_quat(q);
+
+ Eigen::Matrix<SCALAR, 4, 1> someData;
+ someData << kindr_quat.w(), kindr_quat.x(), kindr_quat.y(), kindr_quat.z();
+
+ return someData;
+}
+TEST(KindrJitTest, EigenQuatToKindrRotationQuatTest)
+{
+ using derivativesCppadJIT = ct::core::DerivativesCppadJIT<3, 4>;
+
+ try
+ {
+ // create a function handle (also works for class methods, lambdas, function pointers, ...)
+ typename derivativesCppadJIT::FUN_TYPE_CG f =
+ eigenQuatToKindrRotationQuatFunction<derivativesCppadJIT::CG_SCALAR>;
+
+ // initialize the Auto-Diff Codegen Jacobian
+ derivativesCppadJIT jacCG(f);
+
+ DerivativesCppadSettings settings;
+ settings.createJacobian_ = true;
+
+ // compile the Jacobian
+ jacCG.compileJIT(settings, "EigenQuatToKindrRotationQuatTest");
+
+ } catch (std::exception& e)
+ {
+ std::cout << "Eigen transform test failed: " << e.what() << std::endl;
+ ASSERT_TRUE(false);
+ }
+}
+
+
+//! test the JIT compatibility of kindr Quaternions
+template <typename SCALAR>
+Eigen::Matrix<SCALAR, 4, 1> kindrQuaternionTestFunction(const Eigen::Matrix<SCALAR, 3, 1>& x)
+{
+ kindr::Quaternion<SCALAR> q(x(0), x(1), x(2), x(2));
+
+ Eigen::Matrix<SCALAR, 4, 1> res;
+ res << q.w(), q.x(), q.y(), q.z();
+
+ return res;
+}
+TEST(KindrJitTest, kindrQuaternionTest)
+{
+ using derivativesCppadJIT = ct::core::DerivativesCppadJIT<3, 4>;
+
+ try
+ {
+ // create a function handle (also works for class methods, lambdas, function pointers, ...)
+ typename derivativesCppadJIT::FUN_TYPE_CG f = kindrQuaternionTestFunction<derivativesCppadJIT::CG_SCALAR>;
+
+ // initialize the Auto-Diff Codegen Jacobian
+ derivativesCppadJIT jacCG(f);
+
+ DerivativesCppadSettings settings;
+ settings.createJacobian_ = true;
+
+ // compile the Jacobian
+ jacCG.compileJIT(settings, "kindrQuaternionTestTestLib");
+
+ } catch (std::exception& e)
+ {
+ std::cout << "Eigen transform test failed: " << e.what() << std::endl;
+ ASSERT_TRUE(false);
+ }
+}
+
+
+//! test the JIT compatibility of kindr UnitQuaternions
+template <typename SCALAR>
+Eigen::Matrix<SCALAR, 4, 1> kindrUnitQuaternionTestFunction(const Eigen::Matrix<SCALAR, 3, 1>& x)
+{
+ Eigen::Quaternion<SCALAR> q_init;
+ q_init = Eigen::AngleAxis<SCALAR>(x(0), Eigen::Matrix<SCALAR, 3, 1>::UnitX()) *
+ Eigen::AngleAxis<SCALAR>(x(1), Eigen::Matrix<SCALAR, 3, 1>::UnitY()) *
+ Eigen::AngleAxis<SCALAR>(x(2), Eigen::Matrix<SCALAR, 3, 1>::UnitZ());
+ q_init = Eigen::Quaternion<SCALAR>((SCALAR)1.0, (SCALAR)0, (SCALAR)0, (SCALAR)0);
+
+ kindr::UnitQuaternion<SCALAR> q(q_init);
+
+ Eigen::Matrix<SCALAR, 4, 1> res;
+ res << q.w(), q.x(), q.y(), q.z();
+
+ return res;
+}
+TEST(KindrJitTest, kindrUnitQuaternionTest)
+{
+ using derivativesCppadJIT = ct::core::DerivativesCppadJIT<3, 4>;
+
+ try
+ {
+ // create a function handle (also works for class methods, lambdas, function pointers, ...)
+ typename derivativesCppadJIT::FUN_TYPE_CG f = kindrUnitQuaternionTestFunction<derivativesCppadJIT::CG_SCALAR>;
+
+ // initialize the Auto-Diff Codegen Jacobian
+ derivativesCppadJIT jacCG(f);
+
+ DerivativesCppadSettings settings;
+ settings.createJacobian_ = true;
+
+ // compile the Jacobian
+ jacCG.compileJIT(settings, "kindrUnitQuaternionTestLib");
+
+ } catch (std::exception& e)
+ {
+ std::cout << "Eigen transform test failed: " << e.what() << std::endl;
+ ASSERT_TRUE(false);
+ }
+}
+
+
+/*!
+ * Test JIT compatiblility of the kindr "RotationQuaternion" class
+ */
+template <typename SCALAR>
+Eigen::Matrix<SCALAR, 4, 1> kindrRotationQuaternionTestFunction(const Eigen::Matrix<SCALAR, 3, 1>& x)
+{
+ kindr::RotationQuaternion<SCALAR> q(x(0), x(1), x(2), x(2));
+
+ Eigen::Matrix<SCALAR, 4, 1> res;
+ res << q.w(), q.x(), q.y(), q.z();
+
+ return res;
+}
+TEST(KindrJitTest, kindrRotationQuaternionTest)
+{
+ using derivativesCppadJIT = ct::core::DerivativesCppadJIT<3, 4>;
+
+ try
+ {
+ // create a function handle (also works for class methods, lambdas, function pointers, ...)
+ typename derivativesCppadJIT::FUN_TYPE_CG f =
+ kindrRotationQuaternionTestFunction<derivativesCppadJIT::CG_SCALAR>;
+
+ // initialize the Auto-Diff Codegen Jacobian
+ derivativesCppadJIT jacCG(f);
+
+ DerivativesCppadSettings settings;
+ settings.createJacobian_ = true;
+
+ // compile the Jacobian
+ jacCG.compileJIT(settings, "kindrRotationQuaternionTest");
+
+ } catch (std::exception& e)
+ {
+ std::cout << "Eigen transform test failed: " << e.what() << std::endl;
+ ASSERT_TRUE(false);
+ }
+}
diff --git a/ct_rbd/test/robot/costfunction/rbdJITtests.cpp b/ct_rbd/test/robot/costfunction/rbdJITtests.cpp
new file mode 100644
index 0000000..9263d86
--- /dev/null
+++ b/ct_rbd/test/robot/costfunction/rbdJITtests.cpp
@@ -0,0 +1,16 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#include <ct/rbd/rbd.h>
+#include "rbdJITtests.h"
+
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_rbd/test/robot/costfunction/rbdJITtests.h b/ct_rbd/test/robot/costfunction/rbdJITtests.h
new file mode 100644
index 0000000..a6a305d
--- /dev/null
+++ b/ct_rbd/test/robot/costfunction/rbdJITtests.h
@@ -0,0 +1,264 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <gtest/gtest.h>
+#include "../../models/testhyq/RobCoGenTestHyQ.h"
+
+
+using namespace ct::core;
+using namespace ct::rbd;
+
+bool verbose = true;
+
+
+//! some global variables required for the TaskSpaceTerm test
+const size_t hyqStateDim = 36;
+const size_t hyqControlDim = 12;
+using size_type = ct::core::ADCGScalar;
+using KinTpl_t = TestHyQ::tpl::Kinematics<size_type>;
+KinTpl_t kynTpl;
+size_t eeId = 1;
+
+// penalties for task-space tests
+const Eigen::Matrix<double, 3, 3> Qpos = Eigen::Matrix<double, 3, 3>::Random();
+double Qrot = 1.0;
+
+// terms for task-space tests
+std::shared_ptr<TermTaskspacePosition<KinTpl_t, true, hyqStateDim, hyqControlDim>> termTaskspace(
+ new TermTaskspacePosition<KinTpl_t, true, hyqStateDim, hyqControlDim>(eeId, Qpos, Eigen::Vector3d::Random()));
+
+std::shared_ptr<TermTaskspacePose<KinTpl_t, true, hyqStateDim, hyqControlDim>> termTaskspacePose(
+ new TermTaskspacePose<KinTpl_t, true, hyqStateDim, hyqControlDim>(eeId,
+ Qpos,
+ Qrot,
+ Eigen::Vector3d::Random(),
+ Eigen::Vector3d::Random()));
+
+
+//! test TermTaskSpacePosition term for JIT-compatibility
+template <typename SCALAR>
+Eigen::Matrix<SCALAR, 1, 1> testFunctionTaskSpacePosition(
+ const Eigen::Matrix<SCALAR, hyqStateDim + hyqControlDim, 1> xu)
+{
+ // evaluate the task-space position term
+ Eigen::Matrix<SCALAR, 1, 1> cost;
+ SCALAR t(0.0);
+ cost(0, 0) = termTaskspace->evaluateCppadCg(xu.template head<hyqStateDim>(), xu.template tail<hyqControlDim>(), t);
+ return cost;
+}
+TEST(RBD_JIT_Tests, TaskspacePositionCostFunctionTest)
+{
+ using derivativesCppadJIT = DerivativesCppadJIT<hyqStateDim + hyqControlDim, 1>;
+
+ typename derivativesCppadJIT::FUN_TYPE_CG f = testFunctionTaskSpacePosition<ct::core::ADCGScalar>;
+
+ derivativesCppadJIT jacCG(f);
+
+ DerivativesCppadSettings settings;
+ settings.createJacobian_ = true;
+
+ try
+ {
+ // compile the Jacobian
+ jacCG.compileJIT(settings, "taskSpaceCfTestLib", verbose);
+ std::cout << "testTaskSpacePositionTerm compiled!" << std::endl;
+ } catch (std::exception& e)
+ {
+ std::cout << "testTaskSpacePositionTerm failed!" << std::endl;
+ ASSERT_TRUE(false);
+ }
+}
+
+
+//! test TermTaskSpacePose term for JIT-compatibility
+template <typename SCALAR>
+Eigen::Matrix<SCALAR, 1, 1> testFunctionTaskSpacePose(const Eigen::Matrix<SCALAR, hyqStateDim + hyqControlDim, 1> xu)
+{
+ // evaluate the task-space position term
+ Eigen::Matrix<SCALAR, 1, 1> cost;
+ SCALAR t(0.0);
+ cost(0, 0) =
+ termTaskspacePose->evaluateCppadCg(xu.template head<hyqStateDim>(), xu.template tail<hyqControlDim>(), t);
+ return cost;
+}
+TEST(RBD_JIT_Tests, TaskspacePoseCostFunctionTest)
+{
+ using derivativesCppadJIT = DerivativesCppadJIT<hyqStateDim + hyqControlDim, 1>;
+
+ typename derivativesCppadJIT::FUN_TYPE_CG f = testFunctionTaskSpacePose<ct::core::ADCGScalar>;
+
+ derivativesCppadJIT jacCG(f);
+
+ DerivativesCppadSettings settings;
+ settings.createJacobian_ = true;
+
+ try
+ {
+ // compile the Jacobian
+ jacCG.compileJIT(settings, "taskSpaceCfTestLib", verbose);
+ std::cout << "testTaskSpacePoseTerm compiled!" << std::endl;
+ } catch (std::exception& e)
+ {
+ std::cout << "testTaskSpacePoseTerm failed!" << std::endl;
+ ASSERT_TRUE(false);
+ }
+}
+
+
+//! test the RigidBodyPose class for JIT-compatibility
+template <typename SCALAR>
+Eigen::Matrix<SCALAR, 1, 1> testFunctionRBDPose(const Eigen::Matrix<SCALAR, 3 + 3, 1>& xu)
+{
+ // construct a RigidBodyPose from state vector
+ kindr::Position<SCALAR, 3> pos(xu.template segment<3>(0));
+ kindr::EulerAnglesXyz<SCALAR> euler(xu.template segment<3>(3));
+
+ ct::rbd::tpl::RigidBodyPose<SCALAR> rbdPose;
+ rbdPose.position() = pos;
+ rbdPose.setFromEulerAnglesXyz(euler);
+
+ // we make up some "artificial", non-physical quantity to be auto-diffed:
+ Eigen::Matrix<SCALAR, 1, 1> cost;
+ SCALAR t(0.0);
+
+ cost(0, 0) = rbdPose.position().toImplementation().norm() + rbdPose.getEulerAnglesXyz().toImplementation().norm();
+
+ return cost;
+}
+TEST(RBD_JIT_Tests, RigidBodyPoseTest)
+{
+ using derivativesCppadJIT = DerivativesCppadJIT<3 + 3, 1>;
+
+ typename derivativesCppadJIT::FUN_TYPE_CG f = testFunctionRBDPose<ct::core::ADCGScalar>;
+
+ derivativesCppadJIT jacCG(f);
+
+ DerivativesCppadSettings settings;
+ settings.createJacobian_ = true;
+
+ try
+ {
+ // compile the Jacobian
+ jacCG.compileJIT(settings, "rbdPoseTestLib", verbose);
+ std::cout << "testRBDPose compiled!" << std::endl;
+ } catch (std::exception& e)
+ {
+ std::cout << "testRBDPose failed!" << std::endl;
+ ASSERT_TRUE(false);
+ }
+}
+
+
+//! test the RigidBodyState class for JIT-compatibility
+template <typename SCALAR>
+Eigen::Matrix<SCALAR, 1, 1> testFunctionRigidBodyState(const Eigen::Matrix<SCALAR, 12, 1>& state)
+{
+ // construct a RigidBodyState from state vector
+ ct::rbd::tpl::RigidBodyState<SCALAR> rigidBodyState(
+ ct::rbd::tpl::RigidBodyPose<SCALAR>::EULER); //<- storage type needs to go here in order to let test pass
+
+ rigidBodyState.velocities().getRotationalVelocity().toImplementation() = state.template segment<3>(6);
+ rigidBodyState.velocities().getTranslationalVelocity().toImplementation() = state.template segment<3>(9);
+
+ kindr::Position<SCALAR, 3> pos(state.template segment<3>(0));
+ kindr::EulerAnglesXyz<SCALAR> euler(state.template segment<3>(3));
+
+ rigidBodyState.pose().position() = pos;
+ rigidBodyState.pose().setFromEulerAnglesXyz(euler);
+
+ // helps to check the copy assignment
+ ct::rbd::tpl::RigidBodyState<SCALAR> rigidBodyStateCopy = rigidBodyState;
+
+ // we make up some "artificial", non-physical quantity to be auto-diffed:
+ Eigen::Matrix<SCALAR, 1, 1> cost;
+ SCALAR t(0.0);
+
+ cost(0, 0) = rigidBodyStateCopy.pose().position().toImplementation().norm() +
+ rigidBodyStateCopy.pose().getEulerAnglesXyz().toImplementation().norm() +
+ rigidBodyStateCopy.velocities().getVector().norm();
+
+ return cost;
+}
+TEST(RBD_JIT_Tests, RigidBodyStateTest)
+{
+ using derivativesCppadJIT = DerivativesCppadJIT<12, 1>;
+
+ typename derivativesCppadJIT::FUN_TYPE_CG f = testFunctionRigidBodyState<ct::core::ADCGScalar>;
+
+ derivativesCppadJIT jacCG(f);
+
+ DerivativesCppadSettings settings;
+ settings.createJacobian_ = true;
+
+ try
+ {
+ // compile the Jacobian
+ jacCG.compileJIT(settings, "rbdStateTestLib", verbose);
+ std::cout << "testRigidBodyState compiled!" << std::endl;
+ } catch (std::exception& e)
+ {
+ std::cout << "testRigidBodyState failed!" << std::endl;
+ ASSERT_TRUE(false);
+ }
+}
+
+
+//! test the RBDState class for JIT-compatibility
+template <typename SCALAR>
+Eigen::Matrix<SCALAR, 1, 1> testFunctionRBDState(const Eigen::Matrix<SCALAR, hyqStateDim, 1>& x)
+{
+ // construct an RBDState from data in the state vector.
+ ct::rbd::tpl::RigidBodyState<SCALAR> baseState(
+ ct::rbd::tpl::RigidBodyPose<SCALAR>::EULER); //<- storage type needs to go here in order to let test pass
+
+ baseState.velocities().getRotationalVelocity().toImplementation() = x.template segment<3>(6);
+ baseState.velocities().getTranslationalVelocity().toImplementation() = x.template segment<3>(9);
+
+ kindr::Position<SCALAR, 3> pos(x.template segment<3>(0));
+ kindr::EulerAnglesXyz<SCALAR> euler(x.template segment<3>(3));
+ baseState.pose().position() = pos;
+ baseState.pose().setFromEulerAnglesXyz(euler);
+
+ ct::rbd::tpl::JointState<12, SCALAR> jointState(x.template tail<24>());
+
+ ct::rbd::tpl::RBDState<12, SCALAR> rbdState;
+ rbdState.base() = baseState;
+ rbdState.joints() = jointState;
+
+ // we make up some "artificial", non-physical quantity to be auto-diffed:
+ Eigen::Matrix<SCALAR, 1, 1> cost;
+ SCALAR t(0.0);
+
+ ct::rbd::tpl::RBDState<12, SCALAR> rbdStateCopy = rbdState;
+
+ cost(0, 0) = rbdStateCopy.toStateVectorEulerXyz().norm();
+
+ return cost;
+}
+TEST(RBD_JIT_Tests, RBDStateTest)
+{
+ using derivativesCppadJIT = DerivativesCppadJIT<hyqStateDim, 1>;
+
+ typename derivativesCppadJIT::FUN_TYPE_CG f = testFunctionRBDState<ct::core::ADCGScalar>;
+
+ derivativesCppadJIT jacCG(f);
+
+ DerivativesCppadSettings settings;
+ settings.createJacobian_ = true;
+
+ try
+ {
+ // compile the Jacobian
+ jacCG.compileJIT(settings, "rbdStateTestLib", verbose);
+ std::cout << "testRBDState compiled!" << std::endl;
+ } catch (std::exception& e)
+ {
+ std::cout << "testRBDState failed!" << std::endl;
+ ASSERT_TRUE(false);
+ }
+}
diff --git a/ct_rbd/test/robot/dynamics/DataMapTests.cpp b/ct_rbd/test/robot/dynamics/DataMapTests.cpp
new file mode 100644
index 0000000..2340601
--- /dev/null
+++ b/ct_rbd/test/robot/dynamics/DataMapTests.cpp
@@ -0,0 +1,93 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <memory>
+#include <array>
+
+#include <gtest/gtest.h>
+
+#include <ct/rbd/rbd.h>
+
+#include <ct/rbd/robot/kinematics/RBDDataMap.h>
+#include "../../models/testhyq/RobCoGenTestHyQ.h"
+
+using namespace ct::rbd;
+
+TEST(DataMapTestsHyQ, datamap_test_bool)
+{
+ RBDDataMap<bool, TestHyQ::RobCoGenContainer::NLINKS> ldm_a;
+ RBDDataMap<bool, TestHyQ::RobCoGenContainer::NLINKS> ldm_b;
+
+ for (size_t id = 0; id < TestHyQ::RobCoGenContainer::NLINKS; ++id)
+ {
+ ldm_a[id] = true;
+ ldm_b[id] = false;
+ }
+
+ // check all values
+ for (size_t id = 0; id < TestHyQ::RobCoGenContainer::NLINKS; ++id)
+ {
+ ASSERT_TRUE(ldm_a[id] == true);
+ ASSERT_TRUE(ldm_b[id] == false);
+ ldm_a[id] = ldm_b[id];
+ ASSERT_FALSE(ldm_a[id] == true);
+ }
+
+ // constant assignment
+ ldm_b = true;
+ ASSERT_TRUE(ldm_b[0] == true);
+ ASSERT_TRUE(ldm_b[TestHyQ::RobCoGenContainer::NLINKS - 1] == true);
+
+ // copy from other
+ ldm_b = ldm_a;
+ ASSERT_TRUE(ldm_b[0] == false);
+ ASSERT_TRUE(ldm_b[TestHyQ::RobCoGenContainer::NLINKS - 1] == false);
+
+ // << operator
+ std::cout << ldm_b << std::endl;
+}
+
+TEST(DataMapTestsHyQ, datamap_test_eigen)
+{
+ const size_t numEE = TestHyQ::Kinematics::NUM_EE;
+
+ RBDDataMap<Eigen::Vector3d, numEE> ldm_a;
+ RBDDataMap<Eigen::Vector3d, numEE> ldm_b;
+
+ for (size_t id = 0; id < numEE; ++id)
+ {
+ ldm_a[id] = Eigen::Vector3d::Constant(-888.8);
+ ldm_b[id] = Eigen::Vector3d::Constant(222.2);
+ }
+
+ // check all values
+ for (size_t id = 0; id < numEE; ++id)
+ {
+ ASSERT_TRUE(ldm_a[id](1) == -888.8);
+ ASSERT_TRUE(ldm_b[id](1) == 222.2);
+ ldm_a[id] = ldm_b[id];
+ ASSERT_FALSE(ldm_a[id](1) == -888.8);
+ }
+
+ // constant assignment
+ ldm_b = Eigen::Vector3d::Constant(-888.8);
+ ASSERT_TRUE(ldm_b[0](1) == -888.8);
+ ASSERT_TRUE(ldm_b[numEE - 1](1) == -888.8);
+
+ // copy from other
+ ldm_b = ldm_a;
+ ASSERT_TRUE(ldm_b[0](1) == 222.2);
+ ASSERT_TRUE(ldm_b[numEE - 1](1) == 222.2);
+
+ // << operator
+ std::cout << ldm_b << std::endl;
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_rbd/test/robot/dynamics/DynamicsTests.cpp b/ct_rbd/test/robot/dynamics/DynamicsTests.cpp
new file mode 100644
index 0000000..bd36005
--- /dev/null
+++ b/ct_rbd/test/robot/dynamics/DynamicsTests.cpp
@@ -0,0 +1,68 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <ct/rbd/rbd.h>
+
+#include <memory>
+#include <array>
+
+#include <gtest/gtest.h>
+
+#include "ct/rbd/robot/Dynamics.h"
+#include "ct/rbd/robot/Kinematics.h"
+#include "../../models/testhyq/RobCoGenTestHyQ.h"
+
+using namespace ct::rbd;
+
+TEST(DynamicsTestHyQ, forward_dynamics_test)
+{
+ typedef float valType;
+
+ std::shared_ptr<TestHyQ::tpl::Kinematics<valType>> kyn(new TestHyQ::tpl::Kinematics<valType>);
+
+ typedef TestHyQ::tpl::Dynamics<valType> Dyn;
+
+ Dyn testdynamics(kyn);
+
+ using control_vector_t = typename Dyn::control_vector_t;
+ using ForceVector_t = typename Dyn::ForceVector_t;
+ using RBDState_t = typename Dyn::RBDState_t;
+ using RBDAcceleration_t = typename Dyn::RBDAcceleration_t;
+ using RigidBodyAcceleration_t = typename Dyn::RigidBodyAcceleration_t;
+ using JointAcceleration_t = typename Dyn::JointAcceleration_t;
+ using ExtLinkForces_t = typename Dyn::ExtLinkForces_t;
+ using EE_in_contact_t = typename Dyn::EE_in_contact_t;
+
+ RBDState_t hyq_state;
+ hyq_state.setDefault();
+
+ control_vector_t torque_u = control_vector_t::Zero();
+ ExtLinkForces_t ext_forces;
+ ext_forces = ForceVector_t::Zero();
+
+ RBDAcceleration_t hyq_xd;
+
+ testdynamics.FloatingBaseForwardDynamics(hyq_state, torque_u, ext_forces, hyq_xd);
+
+ RigidBodyAcceleration_t base_a;
+ JointAcceleration_t qdd;
+ testdynamics.FloatingBaseID(hyq_state, qdd, ext_forces, torque_u, base_a);
+
+ ForceVector_t base_w;
+ testdynamics.FloatingBaseFullyActuatedID(hyq_state, base_a, qdd, ext_forces, base_w, torque_u);
+
+ EE_in_contact_t ee_contact = true;
+
+ testdynamics.ProjectedForwardDynamics(ee_contact, hyq_state, torque_u, hyq_xd);
+
+ testdynamics.ProjectedInverseDynamics(ee_contact, hyq_state, hyq_xd, torque_u);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_rbd/test/robot/dynamics/DynamicsTestsFixBase.cpp b/ct_rbd/test/robot/dynamics/DynamicsTestsFixBase.cpp
new file mode 100644
index 0000000..e0bf2f5
--- /dev/null
+++ b/ct_rbd/test/robot/dynamics/DynamicsTestsFixBase.cpp
@@ -0,0 +1,62 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <memory>
+#include <array>
+
+#include <Eigen/Dense>
+#include <gtest/gtest.h>
+
+#include <ct/rbd/rbd.h>
+
+#include "../../models/testIrb4600/RobCoGenTestIrb4600.h"
+#include "ct/rbd/robot/Dynamics.h"
+#include "ct/rbd/robot/Kinematics.h"
+
+using namespace ct::rbd;
+
+TEST(DynamicsTestIrb4600, forward_dynamics_test)
+{
+ std::shared_ptr<TestIrb4600::Kinematics> kyn(new TestIrb4600::Kinematics);
+
+ TestIrb4600::Dynamics testdynamics(kyn);
+
+ using control_vector_t = typename TestIrb4600::Dynamics::control_vector_t;
+ using ForceVector_t = typename TestIrb4600::Dynamics::ForceVector_t;
+ using RBDState_t = typename TestIrb4600::Dynamics::RBDState_t;
+ using RBDAcceleration_t = typename TestIrb4600::Dynamics::RBDAcceleration_t;
+ using JointState_t = typename TestIrb4600::Dynamics::JointState_t;
+ using JointAcceleration_t = typename TestIrb4600::Dynamics::JointAcceleration_t;
+ using ExtLinkForces_t = typename TestIrb4600::Dynamics::ExtLinkForces_t;
+ using EE_in_contact_t = typename TestIrb4600::Dynamics::EE_in_contact_t;
+
+ JointState_t irb_state;
+ irb_state.setZero();
+
+ control_vector_t torque_u = control_vector_t::Zero();
+ ExtLinkForces_t ext_forces;
+ ext_forces = ForceVector_t::Zero();
+
+ JointAcceleration_t irb_xd;
+
+ testdynamics.FixBaseForwardDynamics(irb_state, torque_u, ext_forces, irb_xd);
+
+ JointAcceleration_t qdd;
+ testdynamics.FixBaseID(irb_state, qdd, ext_forces, torque_u);
+
+
+ // EE_in_contact_t ee_contact=true;
+
+ // testdynamics.ProjectedForwardDynamics(ee_contact , irb_state ,torque_u, irb_xd );
+
+ // testdynamics.ProjectedInverseDynamics(ee_contact , irb_state , irb_xd , torque_u);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_rbd/test/robot/kinematics/EEKinematicsTest.cpp b/ct_rbd/test/robot/kinematics/EEKinematicsTest.cpp
new file mode 100644
index 0000000..766e1bc
--- /dev/null
+++ b/ct_rbd/test/robot/kinematics/EEKinematicsTest.cpp
@@ -0,0 +1,338 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <ct/rbd/rbd.h>
+#include <memory>
+
+#include <gtest/gtest.h>
+
+#include <ct/rbd/state/RBDState.h>
+
+#include "../../models/testhyq/RobCoGenTestHyQ.h"
+
+using namespace ct::rbd;
+
+typedef RBDState<12> RBDStateHyQ;
+
+const size_t nFeet = 4;
+
+// Test influence of base angular velocity on feet
+TEST(EEKinematicsTest, testFootVelocityBaseAngularVelocity)
+{
+ RBDStateHyQ state;
+ TestHyQ::Kinematics kinematics;
+
+ // straight pose, all legs streched
+ state.setZero();
+
+ // rotate around x
+ state.baseLocalAngularVelocity()(0) = 1.3;
+
+ for (size_t i = 0; i < nFeet; i++)
+ {
+ kindr::Velocity3D eeVelW;
+ kindr::Velocity3D eeVelB;
+
+ eeVelW = kinematics.getEEVelocityInWorld(i, state);
+ eeVelB = kinematics.getEEVelocityInWorld(i, state);
+
+ // Since world and base are aligned, both velocities should be the same
+ ASSERT_TRUE(eeVelW.toImplementation().isApprox(eeVelB.toImplementation()));
+
+ // x component should be zero
+ ASSERT_NEAR(eeVelW(0), 0.0, 1e-6);
+
+ // y component should be greater zero
+ ASSERT_GT(eeVelW(1), 0.0);
+ }
+}
+
+
+// Test influence of base angular velocity on feet
+TEST(EEKinematicsTest, testFootVelocityBaseZRotation)
+{
+ RBDStateHyQ state;
+ TestHyQ::Kinematics kinematics;
+
+ // random configuration
+ state.setRandom();
+
+ // straight orientation, no joint velocity
+ state.basePose().setFromEulerAnglesXyz(Eigen::Vector3d(0.0, 0.0, 0.0));
+ state.jointVelocities().setZero();
+
+ // rotate around z only
+ state.baseLocalAngularVelocity()(0) = 0.0;
+ state.baseLocalAngularVelocity()(1) = 0.0;
+ state.baseLocalAngularVelocity()(2) = 1.3;
+
+ const size_t nFeet = 4;
+ for (size_t i = 0; i < nFeet; i++)
+ {
+ kindr::Velocity3D eeVelW;
+ kindr::Velocity3D eeVelB;
+ eeVelW = kinematics.getEEVelocityInWorld(i, state);
+ eeVelB = kinematics.getEEVelocityInWorld(i, state);
+
+ // Since world and base are aligned, both velocities should be the same
+ ASSERT_TRUE(eeVelW.toImplementation().isApprox(eeVelB.toImplementation()));
+
+ // z component should be linear velocity in z
+ ASSERT_NEAR(eeVelW(2), state.baseLinearVelocity()(2), 1e-6);
+ }
+}
+
+
+// Test influence of base linear velocity on feet
+TEST(EEKinematicsTest, testFootVelocityBaseLinearVelocity)
+{
+ RBDStateHyQ state;
+ TestHyQ::Kinematics kinematics;
+
+
+ const size_t nTests = 100;
+
+ for (size_t t = 0; t < nTests; t++)
+ {
+ // random configuration
+ state.setRandom();
+
+ // straight orientation, no rotational velocity
+ state.basePose().setFromEulerAnglesXyz(Eigen::Vector3d(0.0, 0.0, 0.0));
+ state.baseLocalAngularVelocity().setZero();
+ state.jointVelocities().setZero();
+
+ const size_t nFeet = 4;
+ for (size_t i = 0; i < nFeet; i++)
+ {
+ kindr::Velocity3D eeVelW;
+ kindr::Velocity3D eeVelB;
+ eeVelW = kinematics.getEEVelocityInWorld(i, state);
+ eeVelB = kinematics.getEEVelocityInWorld(i, state);
+
+ // Since world and base are aligned, both velocities should be the same
+ ASSERT_TRUE(eeVelW.toImplementation().isApprox(eeVelB.toImplementation()));
+
+ // Velocity of feet should be equal to velocity of base
+ ASSERT_TRUE(eeVelW.toImplementation().isApprox(state.baseLinearVelocity().toImplementation()));
+ }
+ }
+}
+
+
+// Test foot positions in base
+TEST(EEKinematicsTest, testFootPositionVaryingBase)
+{
+ RBDStateHyQ state;
+ TestHyQ::Kinematics kinematics;
+
+
+ const size_t nTests = 100;
+
+ for (size_t t = 0; t < nTests; t++)
+ {
+ // random configuration
+ state.setRandom();
+
+ state.jointPositions().setZero();
+
+ const size_t nFeet = 4;
+ std::array<kindr::Position3D, nFeet> B_eePos;
+ for (size_t i = 0; i < nFeet; i++)
+ {
+ B_eePos[i] = kinematics.getEEPositionInBase(i, state.jointPositions());
+
+ // all legs should have same height
+ if (i > 0)
+ ASSERT_NEAR(B_eePos[0](2), B_eePos[i](2), 1e-6);
+
+ // legs should be below belly
+ ASSERT_LT(B_eePos[0](2), -0.5);
+ ASSERT_GT(B_eePos[0](2), -1.5);
+ }
+ }
+}
+
+
+// Test influence of base linear velocity on feet
+TEST(EEKinematicsTest, testFootPositionStraightBase)
+{
+ RBDStateHyQ state;
+ TestHyQ::Kinematics kinematics;
+
+
+ const size_t nTests = 100;
+
+ for (size_t t = 0; t < nTests; t++)
+ {
+ // random configuration
+ state.setRandom();
+
+ // straight orientation, no rotational velocity
+ state.basePose().setFromEulerAnglesXyz(Eigen::Vector3d(0.0, 0.0, 0.0));
+
+ state.jointPositions().setZero();
+
+ const size_t nFeet = 4;
+ std::array<kindr::Position3D, nFeet> W_eePos;
+ for (size_t i = 0; i < nFeet; i++)
+ {
+ W_eePos[i] = kinematics.getEEPositionInWorld(i, state.basePose(), state.jointPositions());
+
+ // all legs should have same height
+ if (i > 0)
+ ASSERT_NEAR(W_eePos[0](2), W_eePos[i](2), 1e-6);
+
+ // legs should be below belly
+ ASSERT_LT(W_eePos[0](2), state.basePose().position()(2) - 0.5);
+ ASSERT_GT(W_eePos[0](2), state.basePose().position()(2) - 1.5);
+ }
+ }
+}
+
+
+// Test influence of base linear velocity on feet
+TEST(EEKinematicsTest, forceMappingTest)
+{
+ RBDStateHyQ state;
+ TestHyQ::Kinematics kinematics;
+
+
+ const size_t nTests = 100;
+
+ for (size_t t = 0; t < nTests; t++)
+ {
+ // random configuration
+ state.setRandom();
+
+ // straight orientation, no rotational velocity
+ state.basePose().setFromEulerAnglesXyz(Eigen::Vector3d(0.0, 0.0, 0.0));
+
+ state.jointPositions().setZero();
+
+ const size_t nFeet = 4;
+
+ std::array<TestHyQ::Kinematics::EEForce, nFeet> eeForcesW;
+
+
+ for (size_t i = 0; i < nFeet; i++)
+ {
+ // only force in z
+ eeForcesW[i].setZero();
+ eeForcesW[i].segment<1>(5).setRandom();
+
+ TestHyQ::Kinematics::EEForce eeForceLink;
+ eeForceLink.setZero();
+
+ eeForceLink = kinematics.mapForceFromWorldToLink(eeForcesW[i], state.basePose(), state.jointPositions(), i);
+
+ // we only expect forces in negative x, no torques
+ for (size_t j = 0; j < 6; j++)
+ {
+ if (j != 3)
+ ASSERT_NEAR(eeForceLink(j), 0.0, 1e-6);
+ }
+
+ ASSERT_NEAR(eeForceLink(3), -eeForcesW[i](5), 1e-6);
+ }
+ }
+}
+
+
+TEST(EEKinematicsTest, forceMagnitudeTest)
+{
+ RBDStateHyQ state;
+ TestHyQ::Kinematics kinematics;
+
+ const size_t nTests = 100;
+
+ for (size_t t = 0; t < nTests; t++)
+ {
+ // random configuration
+ state.setRandom();
+
+ const size_t nFeet = 4;
+ std::array<TestHyQ::Kinematics::EEForce, nFeet> eeForcesW;
+
+ for (size_t i = 0; i < nFeet; i++)
+ {
+ eeForcesW[i].setRandom();
+
+ TestHyQ::Kinematics::EEForce forceLink;
+
+ forceLink = kinematics.mapForceFromWorldToLink(eeForcesW[i], state.basePose(), state.jointPositions(), i);
+
+ // we expect the magnitude not to change
+ ASSERT_NEAR(forceLink.bottomRows<3>().norm(), eeForcesW[i].bottomRows<3>().norm(), 1e-6);
+ }
+ }
+}
+
+
+// Test influence of base linear velocity on feet
+TEST(EEKinematicsTest, torqueMappingTest)
+{
+ RBDStateHyQ state;
+ TestHyQ::Kinematics kinematics;
+
+
+ const size_t nTests = 100;
+
+ for (size_t t = 0; t < nTests; t++)
+ {
+ // random configuration
+ state.setRandom();
+
+ // straight orientation, no rotational velocity
+ state.basePose().setFromEulerAnglesXyz(Eigen::Vector3d(0.0, 0.0, 0.0));
+
+ state.jointPositions().setZero();
+
+ const size_t nFeet = 4;
+
+ std::array<TestHyQ::Kinematics::EEForce, nFeet> eeForcesW;
+
+
+ for (size_t i = 0; i < nFeet; i++)
+ {
+ state.jointPositions()(3 * i + 2) = M_PI / 2.0; // knees bent by 90°
+
+ // only force in z
+ eeForcesW[i].setZero();
+ eeForcesW[i].segment<1>(5).setRandom();
+
+ TestHyQ::Kinematics::EEForce forceLink;
+
+ forceLink = kinematics.mapForceFromWorldToLink(eeForcesW[i], state.basePose(), state.jointPositions(), i);
+
+ // we only expect forces in negative y
+ ASSERT_NEAR(forceLink(3), 0.0, 1e-6);
+ ASSERT_NEAR(forceLink(4), eeForcesW[i](5), 1e-6);
+ ASSERT_NEAR(forceLink(5), 0.0, 1e-6);
+
+ // we expect torques only around z
+ for (size_t j = 0; j < 2; j++)
+ {
+ ASSERT_NEAR(forceLink(j), 0.0, 1e-6);
+ }
+
+ double torqueAbs = std::abs(forceLink(2));
+ ASSERT_NEAR(torqueAbs, forceLink.bottomRows<3>().norm() * 0.33, 1e-6);
+
+ if (i < 2 && (i - 1) % 3 != 0)
+ {
+ ASSERT_NEAR(forceLink.bottomRows<3>().norm(), 0.0, 1e-12);
+ }
+ }
+ }
+}
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_rbd/test/robot/kinematics/KinematicsTest.cpp b/ct_rbd/test/robot/kinematics/KinematicsTest.cpp
new file mode 100644
index 0000000..a7f3668
--- /dev/null
+++ b/ct_rbd/test/robot/kinematics/KinematicsTest.cpp
@@ -0,0 +1,51 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <ct/rbd/rbd.h>
+
+#include <memory>
+#include <array>
+
+#include <gtest/gtest.h>
+
+#include <ct/rbd/state/JointState.h>
+#include "../../models/testhyq/RobCoGenTestHyQ.h"
+#include "../../../include/ct/rbd/robot/Kinematics.h"
+
+#include "ct/rbd/robot/kinematics/EndEffector.h"
+
+using namespace ct;
+using namespace rbd;
+
+
+TEST(TestHyQKinematics, transformTest)
+{
+ TestHyQ::Kinematics kyn;
+ EndEffector<TestHyQ::Kinematics::NJOINTS> eeTest;
+
+ for (size_t i = 0; i < TestHyQ::Kinematics::NUM_EE; i++)
+ {
+ eeTest = kyn.getEndEffector(i);
+ }
+
+ JointState<TestHyQ::Kinematics::NJOINTS> hyqJointState;
+
+ RigidBodyPose hyqPose;
+
+ hyqJointState.setRandom();
+ hyqPose.setRandom();
+
+ size_t ind = 1;
+
+ kindr::Position3D pos = kyn.getEEPositionInWorld(ind, hyqPose, hyqJointState.getPositions());
+}
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_rbd/test/robot/kinematics/KinematicsTestAd.cpp b/ct_rbd/test/robot/kinematics/KinematicsTestAd.cpp
new file mode 100644
index 0000000..af4aa34
--- /dev/null
+++ b/ct_rbd/test/robot/kinematics/KinematicsTestAd.cpp
@@ -0,0 +1,50 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <ct/rbd/rbd.h>
+
+#include <memory>
+#include <array>
+
+#include <gtest/gtest.h>
+
+#include <ct/rbd/state/JointState.h>
+#include "../../models/testhyq/RobCoGenTestHyQ.h"
+#include "../../../include/ct/rbd/robot/Kinematics.h"
+
+#include "ct/rbd/robot/kinematics/EndEffector.h"
+
+using namespace ct;
+using namespace rbd;
+
+
+TEST(TestHyQKinematicsAd, transformTest)
+{
+ typedef float size_type;
+ typedef TestHyQ::tpl::Kinematics<size_type> KinTpl_t;
+ KinTpl_t kynTpl;
+
+ tpl::JointState<TestHyQ::tpl::Kinematics<size_type>::NJOINTS, size_type> hyqJointState;
+ tpl::RigidBodyPose<size_type> hyqPose;
+ Eigen::Matrix<size_type, 3, 1> vec3Tpl;
+
+ hyqPose.setRandom();
+ vec3Tpl.setRandom();
+
+ size_t ind = 1;
+
+ kindr::Position<size_type, 3> pos = kynTpl.getEEPositionInWorld(ind, hyqPose, hyqJointState.getPositions());
+ auto force3d = kynTpl.mapForceFromWorldToLink3d(vec3Tpl, hyqPose, hyqJointState.getPositions(), ind);
+ auto forceLink = kynTpl.mapForceFromWorldToLink(force3d, hyqPose, hyqJointState.getPositions(), ind);
+ auto forceEELink = kynTpl.mapForceFromEEToLink(force3d, hyqPose, hyqJointState.getPositions(), ind);
+}
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_rbd/test/robot/robcogen/RobCoGenContainerTest.cpp b/ct_rbd/test/robot/robcogen/RobCoGenContainerTest.cpp
new file mode 100644
index 0000000..431491d
--- /dev/null
+++ b/ct_rbd/test/robot/robcogen/RobCoGenContainerTest.cpp
@@ -0,0 +1,86 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <ct/rbd/rbd.h>
+
+#include <memory>
+#include <array>
+
+#include <gtest/gtest.h>
+
+#include "ct/rbd/state/JointState.h"
+#include "../../models/testhyq/RobCoGenTestHyQ.h"
+
+TEST(RobCoGenContainerTestHyQ, accessorTest)
+{
+ ct::rbd::TestHyQ::RobCoGenContainer testHyQModel;
+
+ const size_t njoints = ct::rbd::TestHyQ::RobCoGenContainer::NJOINTS;
+
+ ct::rbd::JointState<njoints>::Position jointPos;
+ jointPos.setZero();
+
+ // call random functions
+ testHyQModel.jSim().update(jointPos);
+ testHyQModel.jSim().computeL();
+
+ testHyQModel.inertiaProperties().getTotalMass();
+
+ testHyQModel.homogeneousTransforms().fr_LH_foot_X_fr_trunk(jointPos);
+ testHyQModel.jacobians().fr_trunk_J_LF_hipassemblyCOM(jointPos);
+ testHyQModel.inertiaProperties().getCOM_trunk();
+}
+
+TEST(RobCoGenContainerTestHyQ, transformTest)
+{
+ ct::rbd::TestHyQ::RobCoGenContainer testHyQModel;
+ auto& robcogenTrans = testHyQModel.homogeneousTransforms();
+
+ const size_t njoints = ct::rbd::TestHyQ::RobCoGenContainer::NJOINTS;
+
+ ct::rbd::JointState<njoints>::Position jointPos;
+ jointPos.setRandom();
+
+ typedef ct::rbd::TestHyQ::Kinematics::HomogeneousTransform transform;
+
+ transform Base_LF_hipassembly = testHyQModel.getHomogeneousTransformBaseLinkById(1 + 0, jointPos);
+ transform Base_LF_upperleg = testHyQModel.getHomogeneousTransformBaseLinkById(1 + 1, jointPos);
+ transform Base_LF_lowerleg = testHyQModel.getHomogeneousTransformBaseLinkById(1 + 2, jointPos);
+ transform Base_RF_hipassembly = testHyQModel.getHomogeneousTransformBaseLinkById(1 + 3, jointPos);
+ transform Base_RF_upperleg = testHyQModel.getHomogeneousTransformBaseLinkById(1 + 4, jointPos);
+ transform Base_RF_lowerleg = testHyQModel.getHomogeneousTransformBaseLinkById(1 + 5, jointPos);
+ transform Base_LH_hipassembly = testHyQModel.getHomogeneousTransformBaseLinkById(1 + 6, jointPos);
+ transform Base_LH_upperleg = testHyQModel.getHomogeneousTransformBaseLinkById(1 + 7, jointPos);
+ transform Base_LH_lowerleg = testHyQModel.getHomogeneousTransformBaseLinkById(1 + 8, jointPos);
+ transform Base_RH_hipassembly = testHyQModel.getHomogeneousTransformBaseLinkById(1 + 9, jointPos);
+ transform Base_RH_upperleg = testHyQModel.getHomogeneousTransformBaseLinkById(1 + 10, jointPos);
+ transform Base_RH_lowerleg = testHyQModel.getHomogeneousTransformBaseLinkById(1 + 11, jointPos);
+
+ ASSERT_TRUE(Base_LF_hipassembly == robcogenTrans.fr_trunk_X_fr_LF_hipassembly(jointPos));
+ ASSERT_TRUE(Base_LF_upperleg == robcogenTrans.fr_trunk_X_fr_LF_upperleg(jointPos));
+ ASSERT_TRUE(Base_LF_lowerleg == robcogenTrans.fr_trunk_X_fr_LF_lowerleg(jointPos));
+ ASSERT_TRUE(Base_RF_hipassembly == robcogenTrans.fr_trunk_X_fr_RF_hipassembly(jointPos));
+ ASSERT_TRUE(Base_RF_upperleg == robcogenTrans.fr_trunk_X_fr_RF_upperleg(jointPos));
+ ASSERT_TRUE(Base_RF_lowerleg == robcogenTrans.fr_trunk_X_fr_RF_lowerleg(jointPos));
+ ASSERT_TRUE(Base_LH_hipassembly == robcogenTrans.fr_trunk_X_fr_LH_hipassembly(jointPos));
+ ASSERT_TRUE(Base_LH_upperleg == robcogenTrans.fr_trunk_X_fr_LH_upperleg(jointPos));
+ ASSERT_TRUE(Base_LH_lowerleg == robcogenTrans.fr_trunk_X_fr_LH_lowerleg(jointPos));
+ ASSERT_TRUE(Base_RH_hipassembly == robcogenTrans.fr_trunk_X_fr_RH_hipassembly(jointPos));
+ ASSERT_TRUE(Base_RH_upperleg == robcogenTrans.fr_trunk_X_fr_RH_upperleg(jointPos));
+ ASSERT_TRUE(Base_RH_lowerleg == robcogenTrans.fr_trunk_X_fr_RH_lowerleg(jointPos));
+
+ // test a false one
+ ASSERT_FALSE(Base_RH_lowerleg == Base_LF_hipassembly);
+
+ // test for zero
+ ASSERT_FALSE(Base_RH_lowerleg.isApprox(transform::Zero()));
+}
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_rbd/test/state/JointStateTest.cpp b/ct_rbd/test/state/JointStateTest.cpp
new file mode 100644
index 0000000..bb30cf0
--- /dev/null
+++ b/ct_rbd/test/state/JointStateTest.cpp
@@ -0,0 +1,39 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <ct/core/core.h>
+#include "ct/rbd/state/JointState.h"
+
+#include <gtest/gtest.h>
+
+using namespace ct::rbd;
+
+TEST(JointStateTest, jointLimitTest)
+{
+ JointState<4> js;
+ std::vector<double> lowerLimitVector(4, -1), upperLimitVector(4, 1);
+ Eigen::Vector4d lowerLimitEigen(lowerLimitVector.data()), upperLimitEigen(upperLimitVector.data());
+
+ ASSERT_TRUE(js.checkPositionLimits(lowerLimitVector, upperLimitVector));
+ ASSERT_TRUE(js.checkPositionLimits(lowerLimitEigen, upperLimitEigen));
+
+ js.getPosition(1) = 2;
+ ASSERT_FALSE(js.checkPositionLimits(lowerLimitVector, upperLimitVector));
+ ASSERT_FALSE(js.checkPositionLimits(lowerLimitEigen, upperLimitEigen));
+
+ ASSERT_TRUE(js.checkVelocityLimits(upperLimitVector));
+ ASSERT_TRUE(js.checkVelocityLimits(upperLimitEigen));
+
+ js.getVelocity(1) = 2;
+ ASSERT_FALSE(js.checkVelocityLimits(upperLimitVector));
+ ASSERT_FALSE(js.checkVelocityLimits(upperLimitEigen));
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_rbd/test/state/RigidBodyPoseTest.cpp b/ct_rbd/test/state/RigidBodyPoseTest.cpp
new file mode 100644
index 0000000..cb108a9
--- /dev/null
+++ b/ct_rbd/test/state/RigidBodyPoseTest.cpp
@@ -0,0 +1,212 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <ct/rbd/rbd.h>
+#include "ct/rbd/state/RigidBodyPose.h"
+
+#include <memory>
+#include <array>
+
+#include <gtest/gtest.h>
+
+
+using namespace ct::rbd;
+
+void testEqual(const RigidBodyPose& pose1, const RigidBodyPose& pose2)
+{
+ ASSERT_TRUE(pose1.isNear(pose2));
+ ASSERT_TRUE(pose2.isNear(pose1));
+
+ ASSERT_TRUE(pose2.getEulerAnglesXyz().isNear(pose1.getEulerAnglesXyz(), 1e-10));
+ ASSERT_TRUE(pose2.getRotationQuaternion().isNear(pose1.getRotationQuaternion(), 1e-10));
+
+ ASSERT_TRUE(pose2.getRotationMatrix().isNear(pose1.getRotationMatrix(), 1e-10));
+
+ ASSERT_TRUE(pose2.position().toImplementation().isApprox(pose1.position().toImplementation(), 1e-10));
+
+ ASSERT_TRUE(pose2.computeGravityB().isApprox(pose1.computeGravityB(), 1e-10));
+ ASSERT_TRUE(pose2.computeGravityB6D().isApprox(pose1.computeGravityB6D(), 1e-10));
+}
+
+void testNotEqual(const RigidBodyPose& pose1, const RigidBodyPose& pose2)
+{
+ ASSERT_FALSE(pose1.isNear(pose2));
+ ASSERT_FALSE(pose2.isNear(pose1));
+
+ ASSERT_FALSE(pose2.getEulerAnglesXyz().isNear(pose1.getEulerAnglesXyz(), 1e-10));
+ ASSERT_FALSE(pose2.getRotationQuaternion().isNear(pose1.getRotationQuaternion(), 1e-10));
+
+ ASSERT_FALSE(pose2.getRotationMatrix().isNear(pose1.getRotationMatrix(), 1e-10));
+
+ ASSERT_FALSE(pose2.computeGravityB().isApprox(pose1.computeGravityB(), 1e-10));
+ ASSERT_FALSE(pose2.computeGravityB6D().isApprox(pose1.computeGravityB6D(), 1e-10));
+}
+
+TEST(RigidBodyPoseTest, storageTest)
+{
+ RigidBodyPose poseQuat(RigidBodyPose::QUAT);
+ RigidBodyPose poseEuler(RigidBodyPose::EULER);
+
+ ASSERT_EQ(poseQuat.getStorageType(), RigidBodyPose::QUAT);
+ ASSERT_EQ(poseEuler.getStorageType(), RigidBodyPose::EULER);
+}
+
+TEST(RigidBodyPoseTest, asignmentTest)
+{
+ RigidBodyPose poseQuat(RigidBodyPose::QUAT);
+ RigidBodyPose poseEuler(RigidBodyPose::EULER);
+
+ poseQuat.setRandom();
+ poseEuler.setRandom();
+
+ testNotEqual(poseQuat, poseEuler);
+
+ poseEuler = poseQuat;
+
+ testEqual(poseQuat, poseEuler);
+}
+
+TEST(RigidBodyPoseTest, conversionTest)
+{
+ RigidBodyPose poseQuat(RigidBodyPose::QUAT);
+ RigidBodyPose poseEuler(RigidBodyPose::EULER);
+
+ poseQuat.setRandom();
+ poseEuler.setRandom();
+ poseEuler.setFromEulerAnglesXyz(poseQuat.getEulerAnglesXyz());
+ poseEuler.position() = poseQuat.position();
+
+ testEqual(poseQuat, poseEuler);
+
+ poseQuat.setRandom();
+ poseEuler.setRandom();
+ poseEuler.setFromRotationQuaternion(poseQuat.getRotationQuaternion());
+ poseEuler.position() = poseQuat.position();
+
+ testEqual(poseQuat, poseEuler);
+
+ poseQuat.setIdentity();
+ poseEuler.setIdentity();
+
+ testEqual(poseQuat, poseEuler);
+}
+
+
+TEST(RigidBodyPoseTest, gravityTest)
+{
+ RigidBodyPose poseQuat(RigidBodyPose::QUAT);
+ RigidBodyPose poseEuler(RigidBodyPose::EULER);
+
+ poseQuat.setRandom();
+ poseEuler.setRandom();
+ poseQuat = poseEuler;
+
+ auto gQuat6d = poseQuat.computeGravityB6D();
+ auto gEuler6d = poseEuler.computeGravityB6D();
+
+ ASSERT_TRUE(gQuat6d.head<3>().isApprox(Eigen::Vector3d::Zero()));
+ ASSERT_TRUE(gEuler6d.head<3>().isApprox(Eigen::Vector3d::Zero()));
+
+ // test for orientation 0 0 0
+ poseQuat.setIdentity();
+ poseEuler.setIdentity();
+
+ auto gQuat = poseQuat.computeGravityB();
+ auto gEuler = poseEuler.computeGravityB();
+
+ Eigen::Vector3d gravityW;
+ gravityW << 0.0, 0.0, -9.81;
+
+ ASSERT_TRUE(gQuat.isApprox(gravityW));
+ ASSERT_TRUE(gEuler.isApprox(gravityW));
+
+ // test for 180° rotations
+ for (size_t i = 0; i < 3; i++)
+ {
+ poseQuat.setIdentity();
+ poseEuler.setIdentity();
+
+ // perturb one rotation to 180°
+ Eigen::Vector3d orientationXyz(Eigen::Vector3d::Zero());
+ orientationXyz(i) = M_PI;
+
+ poseQuat.setFromEulerAnglesXyz(orientationXyz);
+ poseEuler.setFromEulerAnglesXyz(orientationXyz);
+
+ // if we perturb in x or y gravity should point upwards
+ if (i < 2)
+ {
+ Eigen::Vector3d gravityUp;
+ gravityUp << 0.0, 0.0, 9.81;
+
+ ASSERT_TRUE(poseQuat.computeGravityB().isApprox(gravityUp));
+ ASSERT_TRUE(poseEuler.computeGravityB().isApprox(gravityUp));
+ }
+ else
+ {
+ // otherwise gravity is normal
+ Eigen::Vector3d gravityDown;
+ gravityDown << 0.0, 0.0, -9.81;
+
+ ASSERT_TRUE(poseQuat.computeGravityB().isApprox(gravityDown));
+ ASSERT_TRUE(poseEuler.computeGravityB().isApprox(gravityDown));
+ }
+ }
+
+ // test for 30° roll
+ poseQuat.setIdentity();
+ poseEuler.setIdentity();
+
+ Eigen::Vector3d orientationXyz(Eigen::Vector3d::Zero());
+ orientationXyz(0) = 30.0 / 180.0 * M_PI;
+
+ poseQuat.setFromEulerAnglesXyz(orientationXyz);
+ poseEuler.setFromEulerAnglesXyz(orientationXyz);
+
+ // x component should be zero
+ ASSERT_NEAR(poseQuat.computeGravityB()(0), 0.0, 1e-10);
+ ASSERT_NEAR(poseEuler.computeGravityB()(0), 0.0, 1e-10);
+
+ // y component should be negative
+ ASSERT_LT(poseQuat.computeGravityB()(1), 0.0);
+ ASSERT_LT(poseEuler.computeGravityB()(1), 0.0);
+
+ // z component should be negative
+ ASSERT_LT(poseQuat.computeGravityB()(2), 0.0);
+ ASSERT_LT(poseEuler.computeGravityB()(2), 0.0);
+}
+
+TEST(RigidBodyPoseTest, rotationTest)
+{
+ RigidBodyPose poseQuat(RigidBodyPose::QUAT);
+ RigidBodyPose poseEuler(RigidBodyPose::EULER);
+
+ Eigen::Vector3d testVector;
+ testVector.setRandom();
+
+ Eigen::Vector3d rotatedVectorQuat;
+ Eigen::Vector3d rotatedVectorEigen;
+
+ Eigen::Vector3d baseVectorQuat = poseQuat.rotateBaseToInertia(testVector);
+ Eigen::Vector3d baseVectorEuler = poseEuler.rotateBaseToInertia(testVector);
+
+ Eigen::Vector3d baseVectorQuatRotMatrix = poseQuat.getRotationMatrix().toImplementation() * testVector;
+ Eigen::Vector3d baseVectorEulerRotMatrix = poseEuler.getRotationMatrix().toImplementation() * testVector;
+
+ ASSERT_TRUE(baseVectorQuat.isApprox(baseVectorEuler, 1e-10));
+ ASSERT_TRUE(baseVectorQuat.isApprox(baseVectorQuatRotMatrix, 1e-10));
+ ASSERT_TRUE(baseVectorQuat.isApprox(baseVectorEulerRotMatrix, 1e-10));
+
+ ASSERT_TRUE(poseQuat.rotateInertiaToBase(baseVectorQuat).isApprox(testVector, 1e-10));
+ ASSERT_TRUE(poseEuler.rotateInertiaToBase(baseVectorEuler).isApprox(testVector, 1e-10));
+}
+
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_rbd/test/systems/FixBaseFDSystemTest.cpp b/ct_rbd/test/systems/FixBaseFDSystemTest.cpp
new file mode 100644
index 0000000..d55e4c7
--- /dev/null
+++ b/ct_rbd/test/systems/FixBaseFDSystemTest.cpp
@@ -0,0 +1,109 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <ct/rbd/rbd.h>
+
+#include <memory>
+#include <array>
+
+#include <iostream>
+
+#include <gtest/gtest.h>
+
+#include "../models/testIrb4600/RobCoGenTestIrb4600.h"
+
+using namespace ct;
+using namespace ct::rbd;
+
+TEST(FixBaseFDSystemTest, ForwardDynamicsTest)
+{
+ const size_t STATE_DIM = FixBaseFDSystem<TestIrb4600::Dynamics>::STATE_DIM;
+
+ std::shared_ptr<core::System<STATE_DIM>> dynamics(new FixBaseFDSystem<TestIrb4600::Dynamics>);
+
+ core::Integrator<STATE_DIM> integrator(dynamics, ct::core::RK4);
+
+ core::StateVector<12> state;
+ state.setZero();
+
+ std::cout << "___________________________________________________________________________" << std::endl;
+
+ std::cout << "Init state: " << state.transpose() << std::endl;
+
+ integrator.integrate_n_steps(state, 0, 1000, 0.001);
+
+ std::cout << "Integrated state: " << state.transpose() << std::endl;
+
+ std::cout << "___________________________________________________________________________" << std::endl;
+}
+
+
+TEST(FixBaseFDSystemTest, ActuatorDynamicsTest)
+{
+ const size_t rbd_state_dim = FixBaseFDSystem<TestIrb4600::Dynamics>::STATE_DIM;
+ const size_t njoints = TestIrb4600::Dynamics::NJOINTS;
+ const size_t actuator_state_dim = 2 * njoints;
+
+ const size_t state_dim = rbd_state_dim + actuator_state_dim;
+
+ // generate actuator dynamics
+ const double w_n = 2;
+ const double zeta = 1; // critical damping
+ const double gc =
+ w_n * w_n; // select oscillator input gain such that we follow the reference input with no amplification
+ std::shared_ptr<ct::rbd::SecondOrderActuatorDynamics<njoints>> actDynamics(
+ new ct::rbd::SecondOrderActuatorDynamics<njoints>(w_n, zeta, gc));
+
+ // generate fix-base dynamics
+ std::shared_ptr<FixBaseFDSystem<TestIrb4600::Dynamics, actuator_state_dim>> combinedDynamics(
+ new FixBaseFDSystem<TestIrb4600::Dynamics, actuator_state_dim>(actDynamics));
+
+ // generate random control action and apply to the system
+ ct::core::ControlVector<njoints> constantControl;
+ constantControl.setRandom();
+ std::cout << "constant control " << constantControl.transpose() << std::endl;
+ std::shared_ptr<ct::core::ConstantController<state_dim, njoints>> constantController(
+ new ct::core::ConstantController<state_dim, njoints>(constantControl));
+ combinedDynamics->setController(constantController);
+
+ // make integrator and integrate the combined system
+ core::Integrator<state_dim> integrator(combinedDynamics, ct::core::RK4);
+
+ core::StateVector<state_dim> state;
+ state.setZero();
+
+ std::cout << "___________________________________________________________________________" << std::endl;
+
+ std::cout << "Init state: " << state.transpose() << std::endl;
+
+ size_t nSteps = 5000;
+ double dt_sim = 0.005;
+ integrator.integrate_n_steps(state, 0, nSteps, dt_sim);
+
+ std::cout << "Integrated overall state: " << state.transpose() << std::endl;
+
+ std::cout << "___________________________________________________________________________" << std::endl;
+
+
+ TestIrb4600::Dynamics::RBDState_t rbdState = combinedDynamics->RBDStateFromVector(state);
+ std::cout << "Integrated joint positions: " << rbdState.joints().getPositions().transpose() << std::endl;
+ std::cout << "Integrated joint velocities: " << rbdState.joints().getVelocities().transpose() << std::endl;
+
+ ct::core::StateVector<actuator_state_dim> actState = combinedDynamics->actuatorStateFromVector(state);
+ std::cout << "Integrated actuator state: " << actState.transpose() << std::endl;
+ std::cout << "___________________________________________________________________________" << std::endl;
+
+ // in the limit, the actuator states must be identical to the constant control
+ for (size_t i = 0; i < njoints; i++)
+ ASSERT_NEAR(actState(i), constantControl(i), 1e-4);
+}
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_rbd/test/systems/FloatingBaseFDSystemTest.cpp b/ct_rbd/test/systems/FloatingBaseFDSystemTest.cpp
new file mode 100644
index 0000000..5c8fde6
--- /dev/null
+++ b/ct_rbd/test/systems/FloatingBaseFDSystemTest.cpp
@@ -0,0 +1,54 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <ct/rbd/rbd.h>
+
+#include <memory>
+#include <array>
+
+#include <gtest/gtest.h>
+
+#include "../models/testhyq/RobCoGenTestHyQ.h"
+
+using namespace ct;
+using namespace ct::rbd;
+
+TEST(FloatingBaseFDSystemTest, forward_dynamics_test)
+{
+ const size_t STATE_DIM_QUAT = FloatingBaseFDSystem<TestHyQ::Dynamics, true>::STATE_DIM;
+ const size_t STATE_DIM_EULER = FloatingBaseFDSystem<TestHyQ::Dynamics, false>::STATE_DIM;
+
+ std::shared_ptr<core::System<STATE_DIM_QUAT>> dynamicsQuat(new FloatingBaseFDSystem<TestHyQ::Dynamics, true>);
+ std::shared_ptr<core::System<STATE_DIM_EULER>> dynamicsEuler(new FloatingBaseFDSystem<TestHyQ::Dynamics, false>);
+
+ core::Integrator<STATE_DIM_QUAT> integratorQuat(dynamicsQuat, ct::core::RK4);
+ core::Integrator<STATE_DIM_EULER> integratorEuler(dynamicsEuler, ct::core::RK4);
+
+ RBDState<12> state;
+ state.setRandom();
+
+ core::StateVector<STATE_DIM_QUAT> stateQuat = state.toStateVectorQuaternion();
+ core::StateVector<STATE_DIM_EULER> stateEuler = state.toStateVectorEulerXyz();
+
+ integratorQuat.integrate_n_steps(stateQuat, 0, 1000, 0.001);
+
+ integratorEuler.integrate_n_steps(stateEuler, 0, 1000, 0.001);
+
+ RBDState<12> finalQuat, finalEuler;
+ finalQuat.fromStateVectorQuaternion(stateQuat);
+ finalEuler.fromStateVectorEulerXyz(stateEuler);
+
+ std::cout << "norm error between quat/euler integration: " << std::endl
+ << (finalQuat.toStateVectorEulerXyz() - finalEuler.toStateVectorEulerXyz()).norm() << std::endl;
+
+ ASSERT_TRUE(finalQuat.isApprox(finalEuler, 1e-6));
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_rbd/test/systems/ProjectedFDSystemTest.cpp b/ct_rbd/test/systems/ProjectedFDSystemTest.cpp
new file mode 100644
index 0000000..8747de5
--- /dev/null
+++ b/ct_rbd/test/systems/ProjectedFDSystemTest.cpp
@@ -0,0 +1,98 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#ifndef TEST_SYSTEMS_PROJECTEDFDSYSTEMTEST_CPP_
+#define TEST_SYSTEMS_PROJECTEDFDSYSTEMTEST_CPP_
+
+#include <ct/rbd/rbd.h>
+#include <memory>
+#include <array>
+
+#include <gtest/gtest.h>
+
+#include "../models/testhyq/RobCoGenTestHyQ.h"
+
+using namespace ct;
+using namespace ct::rbd;
+
+TEST(ProjectedFDSystemTest, projected_forward_dynamics_test)
+{
+ const size_t STATE_DIM_QUAT = ProjectedFDSystem<TestHyQ::Dynamics, true>::STATE_DIM;
+ const size_t STATE_DIM_EULER = ProjectedFDSystem<TestHyQ::Dynamics, false>::STATE_DIM;
+
+ std::shared_ptr<ProjectedFDSystem<TestHyQ::Dynamics, true>> dynamicsQuat(
+ new ProjectedFDSystem<TestHyQ::Dynamics, true>);
+ std::shared_ptr<ProjectedFDSystem<TestHyQ::Dynamics, false>> dynamicsEuler(
+ new ProjectedFDSystem<TestHyQ::Dynamics, false>);
+
+ core::Integrator<STATE_DIM_QUAT> integratorQuat(dynamicsQuat, ct::core::RK4);
+ core::Integrator<STATE_DIM_EULER> integratorEuler(dynamicsEuler, ct::core::RK4);
+
+ TestHyQ::Kinematics kinematics;
+
+ const size_t nTests = 100;
+ const size_t nIntegrationSteps = 50;
+
+ RBDState<12> state;
+
+ for (size_t i = 0; i < nTests; i++)
+ {
+ state.setRandom();
+
+ TestHyQ::Dynamics::EE_in_contact_t contactFlags;
+ Eigen::Matrix<bool, 4, 1> eeContactConfig;
+ eeContactConfig.setRandom();
+
+ for (size_t j = 0; j < 4; j++)
+ {
+ contactFlags[j] = eeContactConfig[j];
+ }
+
+ dynamicsQuat->setEEInContact(contactFlags);
+ dynamicsEuler->setEEInContact(contactFlags);
+
+ core::StateVector<STATE_DIM_QUAT> stateQuat = state.toStateVectorQuaternion();
+ core::StateVector<STATE_DIM_EULER> stateEuler = state.toStateVectorEulerXyz();
+
+ RBDState<12> finalQuat, finalEuler;
+
+ for (size_t j = 0; j < nIntegrationSteps; j++)
+ {
+ integratorQuat.integrate_n_steps(stateQuat, 0, 1, 0.001);
+
+ integratorEuler.integrate_n_steps(stateEuler, 0, 1, 0.001);
+
+ finalQuat.fromStateVectorQuaternion(stateQuat);
+ finalEuler.fromStateVectorEulerXyz(stateEuler);
+
+ // std::cout << "test "<<i<<", integration: "<<j<<std::endl;
+ // std::cout << "diff: " << finalQuat.toStateVectorEulerXyz().transpose() - finalEuler.toStateVectorEulerXyz().transpose()<<std::endl;
+
+ // check if both states are the same
+ ASSERT_TRUE(finalQuat.isApprox(finalEuler, 1e-4));
+
+ // check that EE in contact do not move
+ for (size_t k = 0; k < 4; k++)
+ {
+ if (contactFlags[k])
+ {
+ kinematics.getEEVelocityInWorld(k, finalQuat)
+ .toImplementation()
+ .isApprox(Eigen::Vector3d::Zero(), 1e-6);
+ }
+ }
+ }
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
+
+#endif /* TEST_SYSTEMS_PROJECTEDFDSYSTEMTEST_CPP_ */
diff --git a/ct_rbd/test/systems/linear/RBDLinearizerTest.cpp b/ct_rbd/test/systems/linear/RBDLinearizerTest.cpp
new file mode 100644
index 0000000..1c6ed4d
--- /dev/null
+++ b/ct_rbd/test/systems/linear/RBDLinearizerTest.cpp
@@ -0,0 +1,115 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <ct/rbd/rbd.h>
+
+#include <memory>
+#include <array>
+
+#include <iostream>
+
+#include <gtest/gtest.h>
+
+#include <ct/rbd/systems/linear/RbdLinearizer.h>
+#include "ct/rbd/systems/FixBaseFDSystem.h"
+#include "ct/rbd/systems/FloatingBaseFDSystem.h"
+
+#include "../../models/testIrb4600/RobCoGenTestIrb4600.h"
+#include "../../models/testhyq/RobCoGenTestHyQ.h"
+
+using namespace ct;
+using namespace ct::rbd;
+
+TEST(RBDLinearizerTest, NumDiffComparisonFixedBase)
+{
+ typedef FixBaseFDSystem<TestIrb4600::Dynamics> IrbSystem;
+
+ const size_t STATE_DIM = IrbSystem::STATE_DIM;
+ const size_t CONTROL_DIM = IrbSystem::CONTROL_DIM;
+
+ std::shared_ptr<IrbSystem> irbSystem(new IrbSystem);
+ std::shared_ptr<IrbSystem> irbSystem2(new IrbSystem);
+
+ RbdLinearizer<IrbSystem> rbdLinearizer(irbSystem, true);
+ core::SystemLinearizer<STATE_DIM, CONTROL_DIM> systemLinearizer(irbSystem2, true);
+
+ core::StateVector<STATE_DIM> x;
+ core::ControlVector<CONTROL_DIM> u;
+
+ size_t nTests = 500;
+ for (size_t i = 0; i < nTests; i++)
+ {
+ x.setRandom();
+ u.setRandom();
+
+ auto A_rbd = rbdLinearizer.getDerivativeState(x, u, 0.0);
+ auto B_rbd = rbdLinearizer.getDerivativeControl(x, u, 0.0);
+
+ auto A_system = systemLinearizer.getDerivativeState(x, u, 0.0);
+ auto B_system = systemLinearizer.getDerivativeControl(x, u, 0.0);
+
+ // std::cout << "A_rbd" << std::endl << A_rbd << std::endl << std::endl;
+ // std::cout << "A_system" << std::endl << A_system << std::endl << std::endl;
+ // std::cout << "Diff:" << std::endl << A_rbd-A_system << std::endl << std::endl;
+
+ // std::cout << "B_rbd" << std::endl << B_rbd << std::endl << std::endl;
+ // std::cout << "B_system" << std::endl << B_system << std::endl << std::endl;
+ // std::cout << "Diff:" << std::endl << B_rbd-B_system << std::endl << std::endl;
+
+ ASSERT_LT((A_rbd - A_system).array().abs().maxCoeff(), 1e-5);
+
+ ASSERT_LT((B_rbd - B_system).array().abs().maxCoeff(), 1e-4);
+ }
+}
+
+TEST(RBDLinearizerTest, NumDiffComparisonFloatingBase)
+{
+ typedef FloatingBaseFDSystem<TestHyQ::Dynamics, false, false> HyQSystem;
+
+ const size_t STATE_DIM = HyQSystem::STATE_DIM;
+ const size_t CONTROL_DIM = HyQSystem::CONTROL_DIM;
+
+ std::shared_ptr<HyQSystem> hyqSystem(new HyQSystem);
+ std::shared_ptr<HyQSystem> hyqSystem2(new HyQSystem);
+
+ RbdLinearizer<HyQSystem> rbdLinearizer(hyqSystem, true);
+ core::SystemLinearizer<STATE_DIM, CONTROL_DIM> systemLinearizer(hyqSystem2, true);
+
+ core::StateVector<STATE_DIM> x;
+ core::ControlVector<CONTROL_DIM> u;
+
+ size_t nTests = 500;
+ for (size_t i = 0; i < nTests; i++)
+ {
+ x.setRandom();
+ u.setRandom();
+
+ auto A_rbd = rbdLinearizer.getDerivativeState(x, u, 0.0);
+ auto B_rbd = rbdLinearizer.getDerivativeControl(x, u, 0.0);
+
+ auto A_system = systemLinearizer.getDerivativeState(x, u, 0.0);
+ auto B_system = systemLinearizer.getDerivativeControl(x, u, 0.0);
+
+ // std::cout << "A_rbd" << std::endl << A_rbd << std::endl << std::endl;
+ // std::cout << "A_system" << std::endl << A_system << std::endl << std::endl;
+ //
+ // std::cout << "Diff:" << std::endl << A_rbd-A_system << std::endl << std::endl;
+
+ ASSERT_LT((A_rbd - A_system).array().abs().maxCoeff(), 1e-5);
+
+ // std::cout << "B_rbd" << std::endl << B_rbd << std::endl << std::endl;
+ // std::cout << "B_system" << std::endl << B_system << std::endl << std::endl;
+ // std::cout << "Diff:" << std::endl << B_rbd-B_system << std::endl << std::endl;
+
+ ASSERT_LT((B_rbd - B_system).array().abs().maxCoeff(), 1e-4);
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}