.gitignore

+/build/*
+!/build/.gitkeep
+!.gitkeep
+!.gitignore
+.idea
+
+source/*/Test.h
+
+// ignore Aron files
+*.aron.generated.h
+
+*.bak
+*#
+.#*
+*~
+*.swp
+.*.kate-swp
+.*.swo
+*.pyc
+*.orig
+
+.DS_Store
+CMakeFiles
+CMakeCache.txt
+CMakeLists.txt.user*
+*.autosave
+*~
+
+*.o
+*.os
+*.a
+*.la
+*.lo
+*.so
+*.dylib
+moc_*
+
+# eclipse stuff
+.project
+.pydevproject
+.settings
+.metadata
+.cproject
+.project
+
+# MemoryX
+.cache
+mongod.log*
+data/db/
+data/dbdump/
+
+# Test files
+*.roundtriptest
+
+# Generated Scenario Files
+startScenario.sh
+stopScenario.sh
+ttyACM*.js
+ttyACM*.log
+datapath*.cfg
+
+*.icegrid.xml
+data/RobotAPI/logs
+
+worktree/
+
+etc/python/armarx.egg-info/
+etc/python/build/*
+etc/python/dist/*
+
+.cmake/api/v1/query/cache-v2
+.cmake/api/v1/query/codemodel-v2
+.cmake/api/v1/query/cmakeFiles-v1
+
+# ArViz Recordings
+data/RobotAPI/ArVizStorage/
+
+.vscode/
+.clang-format
+.clang-tidy

.gitlab-ci.yml

+stages:
+  - build-and-test
+  - deploy
+  - post-deploy-test
+
+
+.build-and-test:
+
+  cache:
+    # https://docs.gitlab.com/ee/ci/caching/#share-caches-across-jobs-in-different-branches
+    key: one-key-to-rule-them-all
+    paths:
+      - .apt
+      - .ccache
+
+  before_script:
+    # Apt cache configuration.
+    - rm -rf /var/cache/apt/archives || true
+    - rm -f /etc/apt/apt.conf.d/docker-clean  # Remove docker-clean script to avoid cache deletion.
+    - mkdir .apt || true
+    - ln -s "$CI_PROJECT_DIR/.apt" /var/cache/apt/archives
+
+    # Update apt info.
+    - apt-get update
+
+    # Ccache configuration and introspection.
+    - apt-get install ccache --yes
+    - ccache --set-config=cache_dir="$CI_PROJECT_DIR/.ccache"
+    - ccache --max-size=20G
+    - ccache --show-stats
+
+    # Activate Axii.
+    - source /axii/scripts/install_axii.sh
+    - _axii_auto_env_refresh
+
+  script:
+    - echo "ArmarX Workspace = '$ARMARX_WORKSPACE'"
+
+    # Use workspace configuration from project.
+    - cp "$CI_PROJECT_DIR/.gitlab/ci/armarx-workspace.json" "$ARMARX_WORKSPACE/armarx-workspace.json"
+    - cat "$ARMARX_WORKSPACE/armarx-workspace.json"
+
+    - axii workspace env
+    - _axii_auto_env_refresh
+
+    - echo "Workspace information:"
+    - axii workspace list-modules
+    - axii workspace list-modules --deps
+    - axii workspace info
+
+    - export PROJECT_MODULE="armarx/RobotAPI"
+    - export PROJECT_PATH_IN_WORKSPACE="$armarx__RobotAPI__PATH"
+
+    # Symlink project directory into Axii workspace.
+    - mkdir -p "$(dirname $PROJECT_PATH_IN_WORKSPACE)"
+    - ln -s "$CI_PROJECT_DIR" "$PROJECT_PATH_IN_WORKSPACE"
+
+    # Fix "CMake Error in CMakeLists.txt: Imported target "VirtualRobot" includes non-existent path "/usr/lib/include"
+    # (caused by at least dmp)
+    - mkdir -p /usr/lib/include
+
+    # Upgrade.
+    - axii workspace system --accept-apt-install
+    - axii workspace update --prefer-https
+    - axii workspace upgrade -m "$PROJECT_MODULE"
+
+    - ccache --show-stats
+
+    # Test.
+    # ToDo: Add and use `axii ws test -m "$PROJECT_MODULE"`
+    - cd "$PROJECT_PATH_IN_WORKSPACE/build"
+    - ctest --output-on-failure --output-junit "$CI_PROJECT_DIR/report.xml" .
+
+  artifacts:
+    reports:
+      junit: report.xml
+
+
+build-and-test-bionic:
+  stage: build-and-test
+  extends: .build-and-test
+
+  image: git.h2t.iar.kit.edu:5050/sw/armarx/armarx-gui:latest-bionic
+
+
+build-and-test-jammy:
+  stage: build-and-test
+  extends: .build-and-test
+
+  image: git.h2t.iar.kit.edu:5050/sw/armarx/armarx-gui:latest-jammy
+
+
+docker-bionic:
+  stage: deploy
+  needs: ["build-and-test-bionic"]
+  image:
+    name: gcr.io/kaniko-project/executor:v1.9.0-debug
+    entrypoint: [""]
+  script:
+    - /kaniko/executor
+      --context "${CI_PROJECT_DIR}"
+      --dockerfile "${CI_PROJECT_DIR}/docker/bionic"
+      --destination "${CI_REGISTRY_IMAGE}:latest-bionic"
+
+  rules:
+    - if: $CI_COMMIT_BRANCH == "master"
+
+
+docker-jammy:
+  stage: deploy
+  needs: ["build-and-test-jammy"]
+  image:
+    name: gcr.io/kaniko-project/executor:v1.9.0-debug
+    entrypoint: [""]
+  script:
+    - /kaniko/executor
+      --context "${CI_PROJECT_DIR}"
+      --dockerfile "${CI_PROJECT_DIR}/docker/jammy"
+      --destination "${CI_REGISTRY_IMAGE}:latest-jammy"
+
+  rules:
+    - if: $CI_COMMIT_BRANCH == "master"
+
+
+.test-docker-image-common:
+
+  before_script:
+    - source /axii/scripts/install_axii.sh
+    - _axii_auto_env_refresh
+
+  script:
+    - echo "ArmarX Workspace = '$ARMARX_WORKSPACE'"
+    - printenv
+
+    - axii workspace list-modules
+    - axii workspace list-modules --deps
+    - axii workspace info
+    - echo "RobotAPI directory = '$RobotAPI_DIR'"
+
+    - which armarx
+    - which armarx-package
+
+    - armarx switch docker_test --ice-host 127.0.0.1 --ice-port 10000 --ice-default-host 127.0.0.1 --mongo-host 127.0.0.1 --mongo-port 10001
+    - armarx profile
+    - armarx status || true
+
+    - cd $ArmarXGui_DIR
+    - ctest --output-on-failure .
+
+
+test-docker-image-bionic:
+  stage: post-deploy-test
+  needs: ["docker-bionic"]
+  extends: .test-docker-image-common
+
+  image: git.h2t.iar.kit.edu:5050/sw/armarx/robot-api:latest-bionic
+
+  rules:
+    - if: $CI_COMMIT_BRANCH == "master"
+
+
+test-docker-image-jammy:
+  stage: post-deploy-test
+  needs: ["docker-jammy"]
+  extends: .test-docker-image-common
+
+  image: git.h2t.iar.kit.edu:5050/sw/armarx/robot-api:latest-jammy
+
+  rules:
+    - if: $CI_COMMIT_BRANCH == "master"

.gitlab/CODEOWNERS

+# ARON
+
+/source/RobotAPI/libraries/aron/ @peller @dreher
+
+/source/RobotAPI/interface/aron/ @peller @dreher
+/source/RobotAPI/interface/aron.ice @peller @dreher
+
+
+# ArMem
+/source/RobotAPI/components/armem/ @peller @kartmann @dreher
+
+/source/RobotAPI/libraries/armem/ @peller @kartmann @dreher
+/source/RobotAPI/libraries/armem_robot_localization/ @reister
+/source/RobotAPI/libraries/armem_robot_mapping/ @reister
+
+/source/RobotAPI/interface/armem/ @fratty @RainerKartmann @dreher
+/source/RobotAPI/interface/armem.ice @fratty @RainerKartmann @dreher
+

.gitlab/ci/armarx-workspace.json

+{
+  "modules": {
+    "tools/ccache/default": {},
+    "armarx/RobotAPI": {}
+  },
+  "global": {
+    "prepare": {
+      "cmake": {
+        "definitions": {
+          "CMAKE_BUILD_TYPE": "RelWithDebInfo",
+          "CMAKE_C_COMPILER_LAUNCHER": "$CCACHE",
+          "CMAKE_CXX_COMPILER_LAUNCHER": "$CCACHE"
+        }
+      }
+    }
+  }
+}

CMakeLists.txt

-# HumanoidRobotAPI
+# RobotAPI
+cmake_minimum_required(VERSION 3.10.2)
 
-cmake_minimum_required(VERSION 2.8)
-if (COMMAND cmake_policy)
-    if (POLICY CMP0011)
-        cmake_policy(SET CMP0011 NEW)
-    endif()
+if(POLICY CMP0043)
+    cmake_policy(SET CMP0043 OLD)
 endif()
 
 find_package("ArmarXCore" REQUIRED
-    PATHS "$ENV{HOME}/armarx/Core/build/cmake"
-        "$ENV{HOME}/armarx-install/share/cmake/ArmarXCore"
-        "/org/share/archive/SFB588_RefDist/armarx/share/cmake/ArmarXCore"
+    PATHS "$ENV{HOME}/armarx/Core/build"
 )
 
-include(${ArmarXCore_CMAKE_DIR}/ArmarXProject.cmake)
+include(${ArmarXCore_USE_FILE})
 
-armarx_project("HumanoidRobotAPI")
+set(ARMARX_ENABLE_DEPENDENCY_VERSION_CHECK_DEFAULT TRUE)
+set(ARMARX_ENABLE_AUTO_CODE_FORMATTING TRUE)
 
+armarx_project("RobotAPI")
+depends_on_armarx_package(ArmarXGui)
 
-add_subdirectory(core)
-add_subdirectory(motioncontrol)
-add_subdirectory(applications)
-#add_subdirectory(source)
+find_package(DMP QUIET)
+find_package(OpenCV QUIET)
+find_package(IVT COMPONENTS ivt ivtopencv QUIET)
+find_package(manif QUIET)
 
+add_subdirectory(source)
 
 install_project()
 
+add_subdirectory(scenarios)
+
+
+if (TRUE)
+    find_package(libmongocxx QUIET)
+    find_package(libbsoncxx QUIET)
+    if (NOT libbsoncxx_FOUND OR NOT libbsoncxx_FOUND)
+        string(ASCII 27 Esc)
+        set(BoldYellow  "${Esc}[1;33m")
+        set(ColourReset "${Esc}[m")
+
+        message(WARNING "${BoldYellow}\
+    The packages libbsoncxx or libbsoncxx not found. \
+    They are required to build the memory system (armem and related libraries). \
+    Please use this installation script to install libmongocxx and libbsoncxx:
+    cd ${PROJECT_SOURCE_DIR}/etc/mongocxx/ && ./install_mongocxx.sh ~/repos \
+    or run 'cmake -DCMAKE_PREFIX_PATH=$mongocxx_install_dir' if you have mongocxx already installed \
+    ${ColourReset}")
+    endif()
+endif()

README.md

+# RobotAPI
+
+[Online Documentation](https://armarx.humanoids.kit.edu/RobotAPI-Overview.html)

applications/CMakeLists.txt

-
-
-add_subdirectory(MotionControlTest)
-

applications/MotionControlTest/CMakeLists.txt

-
-armarx_component_set_name(MotionControlTest)
-
-set(COMPONENT_BUILD TRUE)
-
-find_package(Simox QUIET)
-armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
-
-armarx_build_if(COMPONENT_BUILD "component disabled")
-
-# check if ArmarXCoreUnits library gets built
-# LOCATION is NOT-FOUND (equal to FALSE) if library is disabled
-GET_TARGET_PROPERTY(ArmarXCoreUnits_ENABLED ArmarXCoreUnits LOCATION)
-armarx_build_if(ArmarXCoreUnits_ENABLED "ArmarXCoreUnits library disabled")
-
-GET_TARGET_PROPERTY(ArmarXCoreObservers_ENABLED ArmarXCoreObservers LOCATION)
-armarx_build_if(ArmarXCoreObservers_ENABLED "ArmarXCoreObservers library disabled")
-
-
-if (ARMARX_BUILD)
-    set(COMPONENT_LIBS MotionControl HumanoidRobotAPICore ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreRobotStateComponent ArmarXCoreStatechart ArmarXCoreOperations)
-
-    set(SOURCES "")
-    set(HEADERS MotionControlTestApp.h)
-
-    armarx_component_add_executable("${SOURCES}" "${HEADERS}")
-
-endif()

core/CMakeLists.txt

-armarx_set_target("HumanoidRobotAPI Core Library: HumanoidRobotAPICore")
-
-
-find_package(Eigen3 QUIET)
-find_package(Simox QUIET)
-
-armarx_build_if(Eigen3_FOUND "Eigen3 not available")
-armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
-
-if (ARMARX_BUILD)
-    include_directories(${Eigen3_INCLUDE_DIR})
-    include_directories(${VirtualRobot_INCLUDE_DIRS})
-
-    set(LIB_NAME       HumanoidRobotAPICore)
-    set(LIB_VERSION    0.1.0)
-    set(LIB_SOVERSION  0)
-
-    set(LIBS ArmarXInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreRobotStateComponent)
-
-    set(LIB_FILES RobotStatechartContext.cpp)
-    set(LIB_HEADERS RobotStatechartContext.h)
-
-    include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../..)
-    use_ice()
-
-    add_library(${LIB_NAME} SHARED ${LIB_FILES} ${LIB_HEADERS})
-
-    library_settings("${LIB_NAME}" "${LIB_VERSION}" "${LIB_SOVERSION}" "${LIB_HEADERS}")
-    target_link_ice(${LIB_NAME})
-    target_link_libraries(${LIB_NAME} ${LIBS})
-
-
-
-endif()
-

core/RobotStatechartContext.cpp

-/**
-* This file is part of ArmarX.
-*
-* ArmarX is free software; you can redistribute it and/or modify
-* it under the terms of the GNU Lesser General Public License as
-* published by the Free Software Foundation; either version 2 of
-* the License, or (at your option) any later version.
-*
-* ArmarX is distributed in the hope that it will be useful, but
-* WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-* GNU Lesser General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-* @package    Armar4::api
-* @author     Nikolaus Vahrenkamp
-* @date       2012 Nikolaus Vahrenkamp
-* @copyright  http://www.gnu.org/licenses/gpl.txt
-*             GNU General Public License
-*/
-
-#include <Core/core/Component.h>
-#include <Core/core/system/ImportExportComponent.h>
-#include <Core/statechart/Statechart.h>
-#include <Core/robotstate/remote/RemoteRobot.h>
-#include <Core/robotstate/remote/ArmarPose.h>
-#include <Core/robotstate/RobotStateObjectFactories.h>
-
-
-//#include <VirtualRobot/VirtualRobot.h>
-
-#include "RobotStatechartContext.h"
-
-namespace armarx
-{
-    // ****************************************************************
-    // Implementation of Component
-    // ****************************************************************
-    void RobotStatechartContext::onInitComponent()
-    {
-        StatechartContext::onInitComponent();
-        ARMARX_LOG << eINFO << "Init Armar4Context" << flush;
-
-        RobotStateObjectFactories::addFactories(getIceManager()->getCommunicator());
-        kinematicUnitObserverName = getProperty<std::string>("KinematicUnitObserverName").getValue();
-
-
-        usingProxy("RobotStateComponent");
-        usingProxy(kinematicUnitObserverName);
-    }
-
-
-    void RobotStatechartContext::onConnectComponent()
-    {
-        StatechartContext::onConnectComponent();
-        ARMARX_LOG << eINFO << "Starting Armar4Context" << flush;
-
-        // retrieve proxies
-        robotStateComponent = getProxy<RobotStateComponentInterfacePrx>("RobotStateComponent");
-        kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());
-        kinematicUnitObserverPrx = getProxy<KinematicUnitObserverInterfacePrx>(kinematicUnitObserverName);
-        ARMARX_LOG << eINFO << "Fetched proxies" <<  kinematicUnitPrx << " " << robotStateComponent << flush;
-
-		// initialize remote robot	
-        remoteRobot.reset(new RemoteRobot(robotStateComponent->getSynchronizedRobot()));
-        ARMARX_LOG << eINFO << "Created remote robot" << flush;
-    }
-
-    PropertyDefinitionsPtr RobotStatechartContext::createPropertyDefinitions()
-    {
-        return PropertyDefinitionsPtr(new RobotStatechartContextProperties(
-                                               getConfigIdentifier()));
-    }
-
-   /* const VirtualRobot::RobotPtr armarx::Armar4Context::getRobot()
-    {
-        return remoteRobot;
-    }*/
-
-}
-

data/.gitkeep

+Git can only track files and not directory.
+
+Therefore this file is added to all empty directories
+which need to be available after a Git clone.

data/RobotAPI/ArMemStorage/README

+This is the default folder where ArMem stores episodes

data/RobotAPI/ArVizStorage/README

+This is the default folder where ArViz stores recordings

data/RobotAPI/obstacle_avoidance/2d_scene_obstacles.json

+[{
+  "name": "guard_frame_right",
+  "posX": 3100,
+  "posY": 2330,
+  "yaw": 0.0,
+  "axisLengthX": 170,
+  "axisLengthY": 600,
+  "refPosX": 3170,
+  "refPosY": 2320,
+  "safetyMarginX": 300,
+  "safetyMarginY": 300
+},{
+"name": "guard_corner_fix",
+  "posX": 3670,
+  "posY": 2780,
+  "yaw": 0.0,
+  "axisLengthX": 100,
+  "axisLengthY": 150,
+  "refPosX": 3170,
+  "refPosY": 2320,
+  "safetyMarginX": 300,
+  "safetyMarginY": 300
+},{
+  "name": "ctrl_table",
+  "posX": 4400,
+  "posY": 2000,
+  "yaw": 0.0,
+  "axisLengthX": 1500,
+  "axisLengthY": 150,
+  "refPosX": 3170,
+  "refPosY": 2000,
+  "safetyMarginX": 200,
+  "safetyMarginY": 400
+},{
+  "name": "guard_frame_left",
+  "posX": 700,
+  "posY": 2380,
+  "yaw": 0.0,
+  "axisLengthX": 200,
+  "axisLengthY": 650,
+  "refPosX": 500,
+  "refPosY": 2380,
+  "safetyMarginX": 400,
+  "safetyMarginY": 400
+},{
+  "name": "guard_stander",
+  "posX": 305,
+  "posY": 6680,
+  "yaw": -0.23,
+  "axisLengthX": 700,
+  "axisLengthY": 200,
+  "refPosX": 305,
+  "refPosY": 6680,
+  "safetyMarginX": 400,
+  "safetyMarginY": 400
+},{
+  "name": "table_left",
+  "posX": 3700,
+  "posY": 4700,
+  "yaw": 0.75,
+  "axisLengthX": 550,
+  "axisLengthY": 300,
+  "refPosX": 4200,
+  "refPosY": 4900,
+  "safetyMarginX": 300,
+  "safetyMarginY": 300
+},{
+  "name": "table_right",
+  "posX": 4600,
+  "posY": 4700,
+  "yaw": -0.75,
+  "axisLengthX": 550,
+  "axisLengthY": 300,
+  "refPosX": 4200,
+  "refPosY": 4900,
+    "safetyMarginX": 400,
+  "safetyMarginY": 400
+},{
+  "name": "table_corner",
+  "posX": 3400,
+  "posY": 4750,
+  "yaw": 0,
+  "axisLengthX": 800,
+  "axisLengthY": 650,
+  "refPosX": 3400,
+  "refPosY": 5216,
+  "safetyMarginX": 300,
+  "safetyMarginY": 300
+}
+]

data/RobotAPI/robotStateMemoryConfig.txt

+// Configuration set for the Humanoid Robot ARMAR-6
+
+// Reft Hand
+
+sens.Index L 1 Joint.acceleration => HandL
+sens.Index L 1 Joint.gravityTorque  => HandL
+sens.Index L 1 Joint.inertiaTorque  => HandL
+sens.Index L 1 Joint.inverseDynamicsTorque  => HandL
+sens.Index L 1 Joint.position  => HandL
+sens.Index L 1 Joint.torque  => HandL
+sens.Index L 1 Joint.velocity  => HandL
+sens.Index L 2 Joint.acceleration  => HandL
+sens.Index L 2 Joint.gravityTorque  => HandL
+sens.Index L 2 Joint.inertiaTorque  => HandL
+sens.Index L 2 Joint.inverseDynamicsTorque  => HandL
+sens.Index L 2 Joint.position  => HandL
+sens.Index L 2 Joint.torque  => HandL
+sens.Index L 2 Joint.velocity  => HandL
+sens.Index L 3 Joint.acceleration  => HandL
+sens.Index L 3 Joint.gravityTorque  => HandL
+sens.Index L 3 Joint.inertiaTorque  => HandL
+sens.Index L 3 Joint.inverseDynamicsTorque  => HandL
+sens.Index L 3 Joint.position  => HandL
+sens.Index L 3 Joint.torque  => HandL
+sens.Index L 3 Joint.velocity  => HandL
+
+sens.Middle L 1 Joint.acceleration  => HandL
+sens.Middle L 1 Joint.gravityTorque  => HandL
+sens.Middle L 1 Joint.inertiaTorque  => HandL
+sens.Middle L 1 Joint.inverseDynamicsTorque  => HandL
+sens.Middle L 1 Joint.position  => HandL
+sens.Middle L 1 Joint.torque  => HandL
+sens.Middle L 1 Joint.velocity  => HandL
+sens.Middle L 2 Joint.acceleration  => HandL
+sens.Middle L 2 Joint.gravityTorque  => HandL
+sens.Middle L 2 Joint.inertiaTorque  => HandL
+sens.Middle L 2 Joint.inverseDynamicsTorque  => HandL
+sens.Middle L 2 Joint.position  => HandL
+sens.Middle L 2 Joint.torque  => HandL
+sens.Middle L 2 Joint.velocity  => HandL
+sens.Middle L 3 Joint.acceleration  => HandL
+sens.Middle L 3 Joint.gravityTorque  => HandL
+sens.Middle L 3 Joint.inertiaTorque  => HandL
+sens.Middle L 3 Joint.inverseDynamicsTorque  => HandL
+sens.Middle L 3 Joint.position  => HandL
+sens.Middle L 3 Joint.torque  => HandL
+sens.Middle L 3 Joint.velocity  => HandL
+
+sens.Pinky L 1 Joint.acceleration => HandL
+sens.Pinky L 1 Joint.gravityTorque => HandL
+sens.Pinky L 1 Joint.inertiaTorque => HandL
+sens.Pinky L 1 Joint.inverseDynamicsTorque => HandL
+sens.Pinky L 1 Joint.position => HandL
+sens.Pinky L 1 Joint.torque => HandL
+sens.Pinky L 1 Joint.velocity => HandL
+sens.Pinky L 2 Joint.acceleration => HandL
+sens.Pinky L 2 Joint.gravityTorque => HandL
+sens.Pinky L 2 Joint.inertiaTorque => HandL
+sens.Pinky L 2 Joint.inverseDynamicsTorque => HandL
+sens.Pinky L 2 Joint.position => HandL
+sens.Pinky L 2 Joint.torque => HandL
+sens.Pinky L 2 Joint.velocity => HandL
+sens.Pinky L 3 Joint.acceleration => HandL
+sens.Pinky L 3 Joint.gravityTorque => HandL
+sens.Pinky L 3 Joint.inertiaTorque => HandL
+sens.Pinky L 3 Joint.inverseDynamicsTorque => HandL
+sens.Pinky L 3 Joint.position => HandL
+sens.Pinky L 3 Joint.torque => HandL
+sens.Pinky L 3 Joint.velocity => HandL
+
+sens.Ring L 1 Joint.acceleration => HandL
+sens.Ring L 1 Joint.gravityTorque => HandL
+sens.Ring L 1 Joint.inertiaTorque => HandL
+sens.Ring L 1 Joint.inverseDynamicsTorque => HandL
+sens.Ring L 1 Joint.position => HandL
+sens.Ring L 1 Joint.torque => HandL
+sens.Ring L 1 Joint.velocity => HandL
+sens.Ring L 2 Joint.acceleration => HandL
+sens.Ring L 2 Joint.gravityTorque => HandL
+sens.Ring L 2 Joint.inertiaTorque => HandL
+sens.Ring L 2 Joint.inverseDynamicsTorque => HandL
+sens.Ring L 2 Joint.position => HandL
+sens.Ring L 2 Joint.torque => HandL
+sens.Ring L 2 Joint.velocity => HandL
+sens.Ring L 3 Joint.acceleration => HandL
+sens.Ring L 3 Joint.gravityTorque => HandL
+sens.Ring L 3 Joint.inertiaTorque => HandL
+sens.Ring L 3 Joint.inverseDynamicsTorque => HandL
+sens.Ring L 3 Joint.position => HandL
+sens.Ring L 3 Joint.torque => HandL
+sens.Ring L 3 Joint.velocity => HandL
+
+sens.Thumb L 1 Joint.acceleration => HandL
+sens.Thumb L 1 Joint.gravityTorque => HandL
+sens.Thumb L 1 Joint.inertiaTorque => HandL
+sens.Thumb L 1 Joint.inverseDynamicsTorque => HandL
+sens.Thumb L 1 Joint.position => HandL
+sens.Thumb L 1 Joint.torque => HandL
+sens.Thumb L 1 Joint.velocity => HandL
+sens.Thumb L 2 Joint.acceleration => HandL
+sens.Thumb L 2 Joint.gravityTorque => HandL
+sens.Thumb L 2 Joint.inertiaTorque => HandL
+sens.Thumb L 2 Joint.inverseDynamicsTorque => HandL
+sens.Thumb L 2 Joint.position => HandL
+sens.Thumb L 2 Joint.torque => HandL
+sens.Thumb L 2 Joint.velocity => HandL
+
+
+// Right Hand
+
+sens.Index R 1 Joint.acceleration  => HandR
+sens.Index R 1 Joint.gravityTorque  => HandR
+sens.Index R 1 Joint.inertiaTorque  => HandR
+sens.Index R 1 Joint.inverseDynamicsTorque  => HandR
+sens.Index R 1 Joint.position  => HandR
+sens.Index R 1 Joint.torque  => HandR
+sens.Index R 1 Joint.velocity  => HandR
+sens.Index R 2 Joint.acceleration  => HandR
+sens.Index R 2 Joint.gravityTorque  => HandR
+sens.Index R 2 Joint.inertiaTorque  => HandR
+sens.Index R 2 Joint.inverseDynamicsTorque  => HandR
+sens.Index R 2 Joint.position  => HandR
+sens.Index R 2 Joint.torque  => HandR
+sens.Index R 2 Joint.velocity  => HandR
+sens.Index R 3 Joint.acceleration  => HandR
+sens.Index R 3 Joint.gravityTorque  => HandR
+sens.Index R 3 Joint.inertiaTorque  => HandR
+sens.Index R 3 Joint.inverseDynamicsTorque  => HandR
+sens.Index R 3 Joint.position  => HandR
+sens.Index R 3 Joint.torque  => HandR
+sens.Index R 3 Joint.velocity  => HandR
+
+sens.Middle R 1 Joint.acceleration  => HandR
+sens.Middle R 1 Joint.gravityTorque  => HandR
+sens.Middle R 1 Joint.inertiaTorque  => HandR
+sens.Middle R 1 Joint.inverseDynamicsTorque  => HandR
+sens.Middle R 1 Joint.position  => HandR
+sens.Middle R 1 Joint.torque  => HandR
+sens.Middle R 1 Joint.velocity  => HandR
+sens.Middle R 2 Joint.acceleration  => HandR
+sens.Middle R 2 Joint.gravityTorque  => HandR
+sens.Middle R 2 Joint.inertiaTorque  => HandR
+sens.Middle R 2 Joint.inverseDynamicsTorque  => HandR
+sens.Middle R 2 Joint.position  => HandR
+sens.Middle R 2 Joint.torque  => HandR
+sens.Middle R 2 Joint.velocity  => HandR
+sens.Middle R 3 Joint.acceleration  => HandR
+sens.Middle R 3 Joint.gravityTorque  => HandR
+sens.Middle R 3 Joint.inertiaTorque  => HandR
+sens.Middle R 3 Joint.inverseDynamicsTorque  => HandR
+sens.Middle R 3 Joint.position  => HandR
+sens.Middle R 3 Joint.torque  => HandR
+sens.Middle R 3 Joint.velocity  => HandR
+
+sens.Ring R 1 Joint.acceleration => HandR
+sens.Ring R 1 Joint.gravityTorque => HandR
+sens.Ring R 1 Joint.inertiaTorque => HandR
+sens.Ring R 1 Joint.inverseDynamicsTorque => HandR
+sens.Ring R 1 Joint.position => HandR
+sens.Ring R 1 Joint.torque => HandR
+sens.Ring R 1 Joint.velocity => HandR
+sens.Ring R 2 Joint.acceleration => HandR
+sens.Ring R 2 Joint.gravityTorque => HandR
+sens.Ring R 2 Joint.inertiaTorque => HandR
+sens.Ring R 2 Joint.inverseDynamicsTorque => HandR
+sens.Ring R 2 Joint.position => HandR
+sens.Ring R 2 Joint.torque => HandR
+sens.Ring R 2 Joint.velocity => HandR
+sens.Ring R 3 Joint.acceleration => HandR
+sens.Ring R 3 Joint.gravityTorque => HandR
+sens.Ring R 3 Joint.inertiaTorque => HandR
+sens.Ring R 3 Joint.inverseDynamicsTorque => HandR
+sens.Ring R 3 Joint.position => HandR
+sens.Ring R 3 Joint.torque => HandR
+sens.Ring R 3 Joint.velocity => HandR
+
+sens.Thumb R 1 Joint.acceleration => HandR
+sens.Thumb R 1 Joint.gravityTorque => HandR
+sens.Thumb R 1 Joint.inertiaTorque => HandR
+sens.Thumb R 1 Joint.inverseDynamicsTorque => HandR
+sens.Thumb R 1 Joint.position => HandR
+sens.Thumb R 1 Joint.torque => HandR
+sens.Thumb R 1 Joint.velocity => HandR
+sens.Thumb R 2 Joint.acceleration => HandR
+sens.Thumb R 2 Joint.gravityTorque => HandR
+sens.Thumb R 2 Joint.inertiaTorque => HandR
+sens.Thumb R 2 Joint.inverseDynamicsTorque => HandR
+sens.Thumb R 2 Joint.position => HandR
+sens.Thumb R 2 Joint.torque => HandR
+sens.Thumb R 2 Joint.velocity => HandR
+
+sens.Pinky R 1 Joint.acceleration => HandR
+sens.Pinky R 1 Joint.gravityTorque => HandR
+sens.Pinky R 1 Joint.inertiaTorque => HandR
+sens.Pinky R 1 Joint.inverseDynamicsTorque => HandR
+sens.Pinky R 1 Joint.position => HandR
+sens.Pinky R 1 Joint.torque => HandR
+sens.Pinky R 1 Joint.velocity => HandR
+sens.Pinky R 2 Joint.acceleration => HandR
+sens.Pinky R 2 Joint.gravityTorque => HandR
+sens.Pinky R 2 Joint.inertiaTorque => HandR
+sens.Pinky R 2 Joint.inverseDynamicsTorque => HandR
+sens.Pinky R 2 Joint.position => HandR
+sens.Pinky R 2 Joint.torque => HandR
+sens.Pinky R 2 Joint.velocity => HandR
+sens.Pinky R 3 Joint.acceleration => HandR
+sens.Pinky R 3 Joint.gravityTorque => HandR
+sens.Pinky R 3 Joint.inertiaTorque => HandR
+sens.Pinky R 3 Joint.inverseDynamicsTorque => HandR
+sens.Pinky R 3 Joint.position => HandR
+sens.Pinky R 3 Joint.torque => HandR
+sens.Pinky R 3 Joint.velocity => HandR
+
+
+// Arm L
+
+sens.ArmL1_Cla1.acceleration => ArmL
+sens.ArmL1_Cla1.gravityTorque  => ArmL
+sens.ArmL1_Cla1.inertiaTorque  => ArmL
+sens.ArmL1_Cla1.inverseDynamicsTorque  => ArmL
+sens.ArmL1_Cla1.position  => ArmL
+sens.ArmL1_Cla1.torque  => ArmL
+sens.ArmL1_Cla1.velocity  => ArmL
+
+sens.ArmL2_Sho1.acceleration => ArmL
+sens.ArmL2_Sho1.gravityTorque  => ArmL
+sens.ArmL2_Sho1.inertiaTorque  => ArmL
+sens.ArmL2_Sho1.inverseDynamicsTorque  => ArmL
+sens.ArmL2_Sho1.position  => ArmL
+sens.ArmL2_Sho1.torque  => ArmL
+sens.ArmL2_Sho1.velocity  => ArmL
+
+sens.ArmL3_Sho2.acceleration => ArmL
+sens.ArmL3_Sho2.gravityTorque  => ArmL
+sens.ArmL3_Sho2.inertiaTorque  => ArmL
+sens.ArmL3_Sho2.inverseDynamicsTorque  => ArmL
+sens.ArmL3_Sho2.position  => ArmL
+sens.ArmL3_Sho2.torque  => ArmL
+sens.ArmL3_Sho2.velocity  => ArmL
+
+sens.ArmL4_Sho3.acceleration => ArmL
+sens.ArmL4_Sho3.gravityTorque  => ArmL
+sens.ArmL4_Sho3.inertiaTorque  => ArmL
+sens.ArmL4_Sho3.inverseDynamicsTorque  => ArmL
+sens.ArmL4_Sho3.position  => ArmL
+sens.ArmL4_Sho3.torque  => ArmL
+sens.ArmL4_Sho3.velocity  => ArmL
+
+sens.ArmL5_Elb1.acceleration => ArmL
+sens.ArmL5_Elb1.gravityTorque  => ArmL
+sens.ArmL5_Elb1.inertiaTorque  => ArmL
+sens.ArmL5_Elb1.inverseDynamicsTorque  => ArmL
+sens.ArmL5_Elb1.position  => ArmL
+sens.ArmL5_Elb1.torque  => ArmL
+sens.ArmL5_Elb1.velocity  => ArmL
+
+sens.ArmL6_Elb2.acceleration => ArmL
+sens.ArmL6_Elb2.gravityTorque  => ArmL
+sens.ArmL6_Elb2.inertiaTorque  => ArmL
+sens.ArmL6_Elb2.inverseDynamicsTorque  => ArmL
+sens.ArmL6_Elb2.position  => ArmL
+sens.ArmL6_Elb2.torque  => ArmL
+sens.ArmL6_Elb2.velocity  => ArmL
+
+sens.ArmL7_Wri1.acceleration => ArmL
+sens.ArmL7_Wri1.gravityTorque  => ArmL
+sens.ArmL7_Wri1.inertiaTorque  => ArmL
+sens.ArmL7_Wri1.inverseDynamicsTorque  => ArmL
+sens.ArmL7_Wri1.position  => ArmL
+sens.ArmL7_Wri1.torque  => ArmL
+sens.ArmL7_Wri1.velocity  => ArmL
+
+sens.ArmL8_Wri2.acceleration => ArmL
+sens.ArmL8_Wri2.gravityTorque  => ArmL
+sens.ArmL8_Wri2.inertiaTorque  => ArmL
+sens.ArmL8_Wri2.inverseDynamicsTorque  => ArmL
+sens.ArmL8_Wri2.position  => ArmL
+sens.ArmL8_Wri2.torque  => ArmL
+sens.ArmL8_Wri2.velocity  => ArmL
+
+
+// Arm R
+
+sens.ArmR1_Cla1.acceleration => ArmR
+sens.ArmR1_Cla1.gravityTorque  => ArmR
+sens.ArmR1_Cla1.inertiaTorque  => ArmR
+sens.ArmR1_Cla1.inverseDynamicsTorque  => ArmR
+sens.ArmR1_Cla1.position  => ArmR
+sens.ArmR1_Cla1.torque  => ArmR
+sens.ArmR1_Cla1.velocity  => ArmR
+
+sens.ArmR2_Sho1.acceleration => ArmR
+sens.ArmR2_Sho1.gravityTorque  => ArmR
+sens.ArmR2_Sho1.inertiaTorque  => ArmR
+sens.ArmR2_Sho1.inverseDynamicsTorque  => ArmR
+sens.ArmR2_Sho1.position  => ArmR
+sens.ArmR2_Sho1.torque  => ArmR
+sens.ArmR2_Sho1.velocity  => ArmR
+
+sens.ArmR3_Sho2.acceleration => ArmR
+sens.ArmR3_Sho2.gravityTorque  => ArmR
+sens.ArmR3_Sho2.inertiaTorque  => ArmR
+sens.ArmR3_Sho2.inverseDynamicsTorque  => ArmR
+sens.ArmR3_Sho2.position  => ArmR
+sens.ArmR3_Sho2.torque  => ArmR
+sens.ArmR3_Sho2.velocity  => ArmR
+
+sens.ArmR4_Sho3.acceleration => ArmR
+sens.ArmR4_Sho3.gravityTorque  => ArmR
+sens.ArmR4_Sho3.inertiaTorque  => ArmR
+sens.ArmR4_Sho3.inverseDynamicsTorque  => ArmR
+sens.ArmR4_Sho3.position  => ArmR
+sens.ArmR4_Sho3.torque  => ArmR
+sens.ArmR4_Sho3.velocity  => ArmR
+
+sens.ArmR5_Elb1.acceleration => ArmR
+sens.ArmR5_Elb1.gravityTorque  => ArmR
+sens.ArmR5_Elb1.inertiaTorque  => ArmR
+sens.ArmR5_Elb1.inverseDynamicsTorque  => ArmR
+sens.ArmR5_Elb1.position  => ArmR
+sens.ArmR5_Elb1.torque  => ArmR
+sens.ArmR5_Elb1.velocity  => ArmR
+
+sens.ArmR6_Elb2.acceleration => ArmR
+sens.ArmR6_Elb2.gravityTorque  => ArmR
+sens.ArmR6_Elb2.inertiaTorque  => ArmR
+sens.ArmR6_Elb2.inverseDynamicsTorque  => ArmR
+sens.ArmR6_Elb2.position  => ArmR
+sens.ArmR6_Elb2.torque  => ArmR
+sens.ArmR6_Elb2.velocity  => ArmR
+
+sens.ArmR7_Wri1.acceleration => ArmR
+sens.ArmR7_Wri1.gravityTorque  => ArmR
+sens.ArmR7_Wri1.inertiaTorque  => ArmR
+sens.ArmR7_Wri1.inverseDynamicsTorque  => ArmR
+sens.ArmR7_Wri1.position  => ArmR
+sens.ArmR7_Wri1.torque  => ArmR
+sens.ArmR7_Wri1.velocity  => ArmR
+
+sens.ArmR8_Wri2.acceleration => ArmR
+sens.ArmR8_Wri2.gravityTorque  => ArmR
+sens.ArmR8_Wri2.inertiaTorque  => ArmR
+sens.ArmR8_Wri2.inverseDynamicsTorque  => ArmR
+sens.ArmR8_Wri2.position  => ArmR
+sens.ArmR8_Wri2.torque  => ArmR
+sens.ArmR8_Wri2.velocity  => ArmR
+
+
+// Neck
+
+sens.Neck_1_Yaw.acceleration => Neck
+sens.Neck_1_Yaw.gravityTorque => Neck
+sens.Neck_1_Yaw.inertiaTorque => Neck
+sens.Neck_1_Yaw.inverseDynamicsTorque => Neck
+sens.Neck_1_Yaw.position => Neck
+sens.Neck_1_Yaw.torque => Neck
+sens.Neck_1_Yaw.velocity => Neck
+
+sens.Neck_2_Pitch.acceleration => Neck
+sens.Neck_2_Pitch.gravityTorque => Neck
+sens.Neck_2_Pitch.inertiaTorque => Neck
+sens.Neck_2_Pitch.inverseDynamicsTorque => Neck
+sens.Neck_2_Pitch.position => Neck
+sens.Neck_2_Pitch.torque => Neck
+sens.Neck_2_Pitch.velocity => Neck
+
+
+// Torso
+
+sens.TorsoJoint.acceleration => Torso
+sens.TorsoJoint.gravityTorque  => Torso
+sens.TorsoJoint.inertiaTorque  => Torso
+sens.TorsoJoint.inverseDynamicsTorque  => Torso
+sens.TorsoJoint.position  => Torso
+sens.TorsoJoint.torque  => Torso
+sens.TorsoJoint.velocity  => Torso
+
+
+// Platform
+
+sens.Platform.accelerationX => Platform
+sens.Platform.accelerationY => Platform
+sens.Platform.accelerationRotation => Platform
+sens.Platform.absolutePositionX => Platform
+sens.Platform.absolutePositionY => Platform
+sens.Platform.absolutePositionRotation => Platform
+sens.Platform.relativePositionX => Platform
+sens.Platform.relativePositionY => Platform
+sens.Platform.relativePositionRotation => Platform
+sens.Platform.velocityX  => Platform
+sens.Platform.velocityY  => Platform
+sens.Platform.velocityRotation  => Platform
+
+
+// FT L
+
+sens.FT L.fx => FT L
+sens.FT L.fy => FT L
+sens.FT L.fz => FT L
+sens.FT L.gravCompFx => FT L
+sens.FT L.gravCompFy => FT L
+sens.FT L.gravCompFz => FT L
+sens.FT L.gravCompTx => FT L
+sens.FT L.gravCompTy => FT L
+sens.FT L.gravCompTz => FT L
+sens.FT L.tx => FT L
+sens.FT L.ty => FT L
+sens.FT L.tz => FT L
+
+
+// FT R
+
+sens.FT R.fx => FT R
+sens.FT R.fy => FT R
+sens.FT R.fz => FT R
+sens.FT R.gravCompFx => FT R
+sens.FT R.gravCompFy => FT R
+sens.FT R.gravCompFz => FT R
+sens.FT R.gravCompTx => FT R
+sens.FT R.gravCompTy => FT R
+sens.FT R.gravCompTz => FT R
+sens.FT R.tx => FT R
+sens.FT R.ty => FT R
+sens.FT R.tz => FT R

data/RobotAPI/robots/Armar3/ArmarIII-Head.xml

+<?xml version="1.0" encoding="UTF-8" ?>
+
+<Robot Type="ArmarIII Head" RootNode="Head Base">
+    
+    <RobotNode name="Head Base">
+        <Transform>
+            <DH theta="90" d="0" a="0" alpha="0" unitsangle="degree" unitslength="mm"/>
+        </Transform>
+                
+        <Visualization>
+            <File type="Inventor">fullmodel/head_base.wrl</File>
+        </Visualization>
+        
+        <Physics>
+            <CoM location="VisualizationBBoxCenter"/>
+            <Mass value="4" units="kg" />
+        </Physics>
+        
+        <CollisionModel>
+            <File type="Inventor">convexModel/head_base.wrl</File>
+        </CollisionModel>   
+
+        <Child name="Neck_1_Pitch"/>
+    </RobotNode>
+
+    <RobotNode name="Neck_1_Pitch">
+        <Transform>
+            <DH theta="0" d="0" a="0" alpha="90" unitsangle="degree" unitslength="mm"/>
+            <DH theta="90" d="0" a="0" alpha="0" unitsangle="degree" unitslength="mm"/>
+        </Transform>
+                
+
+        <Joint type="revolute">
+            <!--DH theta="90" d="0" a="0" alpha="90" unitsangle="degree" unitslength="mm"/-->
+            <axis x="0" y="0" z="-1"/>
+            <Limits unit="degree" lo="-45" hi="37"/>
+            <MaxVelocity unit="radian" value="1"/>
+            <MaxAcceleration value="10"/>
+            <MaxTorque value="3000"/>
+        </Joint>
+
+        <Visualization>
+            <File type="Inventor">fullmodel/neck_pitch_link.wrl</File>           
+        </Visualization>
+        
+        <Physics>
+            <CoM location="VisualizationBBoxCenter"/>
+            <Mass value="4" units="kg" />
+            <IgnoreCollision name="Hip Yaw"/>
+        </Physics>
+        
+        <CollisionModel>
+            <File type="Inventor">convexModel/neck_pitch_link.wrl</File>
+        </CollisionModel>
+        
+        <Child name="Neck_2_Roll"/>
+    </RobotNode>
+        
+    <RobotNode name="Neck_2_Roll">
+        <Transform>
+			<DH a="0" d="0" theta="0" alpha="90" unitsangle="degree" unitslength="mm"/>
+			<DH a="0" d="0" theta="90" alpha="0" unitsangle="degree" unitslength="mm"/>
+		</Transform>
+        <Joint type="revolute">
+            <!--DH theta="90" d="0" a="0" alpha="90" unitsangle="degree" unitslength="mm"/-->
+            <axis x="0" y="0" z="1"/>
+            <Limits unit="degree" lo="-45" hi="45"/>
+            <MaxVelocity unit="radian" value="1"/>
+            <MaxAcceleration value="10"/>
+            <MaxTorque value="3000"/>
+        </Joint>
+        
+        <Visualization>
+            <File type="Inventor">fullmodel/neck_roll_link.wrl</File>
+        </Visualization>
+                
+        <Physics>
+            <CoM location="VisualizationBBoxCenter"/>
+            <Mass value="2" units="kg" />
+            <IgnoreCollision name="Hip Yaw"/>
+            <IgnoreCollision name="Head Base"/>
+        </Physics>
+        
+        <CollisionModel>
+            <File type="Inventor">convexModel/neck_roll_link.wrl</File>
+        </CollisionModel>
+        
+        <Child name="Neck_3_Yaw"/>
+    </RobotNode>    
+    
+    <RobotNode name="Neck_3_Yaw">
+       <Transform>
+			<DH a="0" d="0" theta="0" alpha="90" unitsangle="degree" unitslength="mm"/>
+			<DH a="0" d="0" theta="90" alpha="0" unitsangle="degree" unitslength="mm"/>
+		</Transform>
+        <Joint type="revolute">
+            <!--DH theta="90" d="120" a="0"  alpha="90" unitsangle="degree" unitslength="mm"/-->
+            <axis x="0" y="0" z="-1"/>
+            <Limits unit="degree" lo="-120" hi="120"/>
+            <MaxVelocity unit="radian" value="1"/>
+            <MaxAcceleration value="10"/>
+            <MaxTorque value="3000"/>
+        </Joint>
+        
+        <Visualization>
+            <File type="Inventor">fullmodel/neck_yaw_link.wrl</File>
+        </Visualization>
+                
+        <Physics>
+            <CoM location="VisualizationBBoxCenter"/>
+            <Mass value="2" units="kg" />
+        </Physics>
+        
+        <CollisionModel>
+            <File type="Inventor">convexModel/neck_yaw_link.wrl</File>
+        </CollisionModel>
+        <PrimitiveModel>
+            <Primitives>
+                <Sphere radius="140">
+                    <Transform>
+                        <Translation x="40" y="0" z="180" />
+                    </Transform>
+                </Sphere>
+            </Primitives>
+        </PrimitiveModel>
+
+        <Child name="Head_Tilt"/>
+    </RobotNode>
+
+    <RobotNode name="Head_Tilt">
+        <Transform>
+			<DH theta="0" d="120" a="0" alpha="90" unitsangle="degree" unitslength="mm"/>
+		</Transform>
+        <Joint type="revolute">
+            <!--DH theta="0" d="0" a="0"  alpha="-90" unitsangle="degree" unitslength="mm"/-->
+            <axis x="0" y="0" z="1"/>
+            <!--<Limits unit="degree" lo="-45" hi="45"/>/-->
+            <Limits unit="degree" lo="0" hi="0"/>
+            <MaxVelocity unit="radian" value="1"/>
+            <MaxAcceleration value="10"/>
+            <MaxTorque value="3000"/>
+        </Joint>
+        
+        <Physics>
+            <CoM location="VisualizationBBoxCenter"/>
+            <Mass value="6.79877000" units="kg" />
+            <IgnoreCollision name="Neck_2_Roll"/>
+            <IgnoreCollision name="Hip Yaw"/>
+        </Physics>
+        
+        <Visualization>
+            <File type="Inventor">fullmodel/head.wrl</File>
+        </Visualization>
+        
+        <CollisionModel>
+            <File type="Inventor">convexModel/head.wrl</File>
+        </CollisionModel>  
+         
+        <Child name="Head Center1"/>
+    </RobotNode>
+    
+    <RobotNode name="Head Center1">
+        <Transform>
+			<DH theta="0" d="0" a="0" alpha="-90" unitsangle="degree" unitslength="mm"/>
+		</Transform>
+        <Child name="Head Center"/>
+    </RobotNode>
+ 
+ 
+    <RobotNode name="Head Center">
+        <Transform>
+			<DH theta="0" d="54.5" a="0" alpha="-90" unitsangle="degree" unitslength="mm"/>
+		</Transform>
+        <Child name="Cameras"/>
+        <Child name="Jaw"/>
+        <Child name="Kinect"/>
+        <Child name="DepthCamera"/>
+        <Child name="DepthCameraSim"/>
+        <Child name="HeadIMU"/>
+    </RobotNode>
+
+    <RobotNode name="Jaw"> 
+        <Joint type="revolute"> 
+            <axis x="0" y="0" z="1"/>
+            <Limits unit="degree" lo="-30" hi="30"/>
+            <MaxVelocity unit="radian" value="1"/>
+            <MaxAcceleration value="10"/>
+            <MaxTorque value="3000"/>
+        </Joint>
+        <Visualization> 
+            <File type="Inventor">fullmodel/jaw.wrl</File>
+        </Visualization>
+    </RobotNode>
+
+    <RobotNode name="Cameras">
+        <Transform>
+            <Translation x="100" y="0" z="0" units='mm'/>
+        </Transform>
+        <Joint type="revolute">
+            <Axis x="0" y="0" z="-1"/>
+            <Limits unit="degree" lo="-20" hi="5"/> <!-- Motor encoder gives values until -24, but eyes only move until -20 or -21 -->
+            <MaxVelocity unit="radian" value="1"/>
+            <MaxAcceleration value="10"/>
+            <MaxTorque value="3000"/>
+        </Joint>
+
+        <CollisionModel>
+            <File type="Inventor">iv_primitives/cyl2.wrl</File>
+        </CollisionModel> 
+
+        <Physics>
+            <CoM location="VisualizationBBoxCenter"/>
+            <Mass value="0.2" units="kg" />
+            <IgnoreCollision name="Neck_2_Roll"/>
+            <IgnoreCollision name="Hip Yaw"/>            
+	    <IgnoreCollision name="Head_Tilt"/>
+        </Physics>
+
+        <Child name="Eye_Left_Dummy"/>
+        <Child name="Eye_Right_Dummy"/>
+        <Child name="VirtualCentralGaze"/>
+    </RobotNode>
+
+    <RobotNode name="VirtualCentralGaze">
+        <Joint type="prismatic">
+            <TranslationDirection x="1" y="0" z="0"/>
+            <Limits unit="mm" lo="0" hi="10000"/>
+            <MaxVelocity unit="radian" value="1"/>
+            <MaxAcceleration value="10"/>
+            <MaxTorque value="3000"/>
+        </Joint>
+        <Visualization enable="true">
+            <CoordinateAxis type="Inventor" enable="false" scaling="3"/>
+        </Visualization>
+    </RobotNode>
+
+    <RobotNode name="Kinect">
+        <Transform>
+            <Translation x="140" y="-100" z="0" units='mm'/>
+        </Transform>
+        <Joint type="prismatic">
+            <TranslationDirection x="1" y="0" z="0"/>
+            <Limits unit="mm" lo="0" hi="10000"/>
+            <MaxVelocity unit="radian" value="1"/>
+            <MaxAcceleration value="10"/>
+            <MaxTorque value="3000"/>
+        </Joint>
+        <Visualization enable="true">
+            <CoordinateAxis type="Inventor" enable="false" scaling="3"/>
+        </Visualization>
+    </RobotNode>
+
+    <RobotNode name="DepthCamera">
+        <Transform>
+            <Translation x="105" y="90" z="0" units='mm'/>
+            <rollpitchyaw roll="0" pitch="90" yaw="0" unitsAngle="degree"/>
+            <rollpitchyaw roll="0" pitch="0" yaw="180" unitsAngle="degree"/>
+            <rollpitchyaw roll="21.0" pitch="0" yaw="0" unitsAngle="degree"/>
+            <rollpitchyaw roll="0" pitch="0" yaw="180" unitsAngle="degree"/>
+        </Transform>
+        
+        <Visualization enable="true">
+            <CoordinateAxis type="Inventor" enable="false" scaling="3"/>
+        </Visualization>
+    </RobotNode>
+
+    <RobotNode name="DepthCameraSim">
+        <Transform>
+            <Translation x="105" y="70" z="0" units='mm'/>
+            <rollpitchyaw roll="0" pitch="90" yaw="0" unitsAngle="degree"/>
+            <rollpitchyaw roll="0" pitch="0" yaw="180" unitsAngle="degree"/>
+            <rollpitchyaw roll="20.7" pitch="0" yaw="0" unitsAngle="degree"/>
+            <rollpitchyaw roll="0" pitch="0" yaw="270" unitsAngle="degree"/>
+        </Transform>
+        
+        <Visualization enable="true">
+            <CoordinateAxis type="Inventor" enable="false" scaling="3"/>
+        </Visualization>
+    </RobotNode>
+
+    <RobotNode name="HeadIMU">
+        <Transform>
+            <Translation x="0" y="-80" z="0" units='mm'/>
+            <rollpitchyaw roll="90" pitch="0" yaw="0" unitsAngle="degree"/>
+            <rollpitchyaw roll="0" pitch="0" yaw="180" unitsAngle="degree"/>
+        </Transform>
+        
+        <Visualization enable="true">
+            <CoordinateAxis type="Inventor" enable="false" scaling="3"/>
+        </Visualization>
+    </RobotNode>
+
+
+    
+    <RobotNode name="Eye_Left_Dummy">
+        <Transform>
+          <DH a="0" d="46.5" theta="0" alpha="-90" unitsangle="degree" unitslength="mm"/>
+        </Transform>
+        <Child name="Eye_Left"/>
+    </RobotNode>
+
+    <RobotNode name="Eye_Right_Dummy">
+        <Transform>
+          <DH a="0" d="-46.5" theta="0" alpha="-90" unitsangle="degree" unitslength="mm"/>
+        </Transform>
+        <Child name="Eye_Right"/>
+      </RobotNode>
+
+    <RobotNode name="Eye_Left">
+        <Joint type="revolute">
+          <Axis x="0" y="0" z="1"/>
+          <Limits unit="degree" lo="-30" hi="30"/>
+            <MaxVelocity unit="radian" value="1"/>
+            <MaxAcceleration value="10"/>
+            <MaxTorque value="3000"/>
+        </Joint>
+        <Visualization>
+          <File type="Inventor">fullmodel/eye_l.wrl</File>
+        </Visualization>
+
+
+        <CollisionModel>
+            <File type="Inventor">iv_primitives/cyl1.wrl</File>
+        </CollisionModel> 
+
+        <Physics>
+            <CoM location="VisualizationBBoxCenter"/>
+            <Mass value="0.1" units="kg" />
+            <IgnoreCollision name="Cameras"/>            
+	    <IgnoreCollision name="Head_Tilt"/>
+        </Physics>
+
+        <Child name="Lid Left Bottom"/>
+        <Child name="Lid Left Top"/>
+        <Child name="EyeLeftCameraPreTransformation"/>
+    </RobotNode>
+
+    <RobotNode name="Eye_Right">
+        <Joint type="revolute">
+          <Axis x="0" y="0" z="1"/>
+          <Limits unit="degree" lo="-30" hi="30"/>
+            <MaxVelocity unit="radian" value="1"/>
+            <MaxAcceleration value="10"/>
+            <MaxTorque value="3000"/>
+        </Joint>
+        <Visualization>
+          <File type="Inventor">fullmodel/eye_r.wrl</File>
+        </Visualization>
+
+        <CollisionModel>
+            <File type="Inventor">iv_primitives/cyl1.wrl</File>
+        </CollisionModel> 
+
+        <Physics>
+            <CoM location="VisualizationBBoxCenter"/>
+            <Mass value="0.1" units="kg" />
+            <IgnoreCollision name="Cameras"/>            
+	    <IgnoreCollision name="Head_Tilt"/>
+        </Physics>
+
+        <Child name="Lid Right Bottom"/>
+        <Child name="Lid Right Top"/>
+        <Child name="EyeRightCameraPreTransformation"/>
+    </RobotNode>
+
+    <RobotNode name="EyeLeftCameraPreTransformation">
+        <Transform>
+            <translation x="20" y="0" z="0"/>
+            <RollPitchYaw roll="0" pitch="90" yaw ="0" units="degree"/>
+        </Transform>
+        <Child name="EyeLeftCamera"/>
+        <Child name="EyeLeftCameraSim"/>
+    </RobotNode>
+
+    <RobotNode name="EyeLeftCamera">
+        <Transform>
+            <RollPitchYaw roll="0" pitch="0" yaw ="90" units="degree"/>
+        </Transform>
+	<Visualization enable="true">
+                <CoordinateAxis type="Inventor" enable="false" scaling="5"/>
+	</Visualization>
+        <Child name="HeadMotionMeasurementTCP"/>
+        <!--Sensor type="Camera" name="LeftEyeCam">
+        </Sensor-->
+    </RobotNode>
+
+    <RobotNode name="EyeLeftCameraSim">
+        <Transform>
+            <RollPitchYaw roll="0" pitch="0" yaw ="180" units="degree"/>
+        </Transform>
+    </RobotNode>
+    
+    <RobotNode name="HeadMotionMeasurementTCP">
+        <Transform>
+            <translation x="0" y="0" z="1000"/>
+        </Transform>
+    </RobotNode>
+
+  <RobotNode name="Lid Left Bottom">
+    <Joint type="revolute">
+      <Axis x="0" y="1" z="0"/>
+        <MaxVelocity unit="radian" value="1"/>
+        <MaxAcceleration value="10"/>
+        <MaxTorque value="3000"/>
+    </Joint>
+    <Visualization>
+      <File type="Inventor">fullmodel/lid_l_bottom.wrl</File>
+    </Visualization>
+  </RobotNode>
+  
+  <RobotNode name="Lid Left Top">
+    <Joint type="revolute">
+      <Axis x="0" y="1" z="0"/>
+    </Joint>
+    <Visualization>
+      <File type="Inventor">fullmodel/lid_l_top.wrl</File>
+    </Visualization>
+  </RobotNode>
+
+    <RobotNode name="EyeRightCameraPreTransformation">
+        <Transform>
+            <translation x="20" y="0" z="0"/>
+            <RollPitchYaw roll="0" pitch="90" yaw ="0" units="degree"/>
+        </Transform>
+        <Child name="EyeRightCamera"/>
+        <Child name="EyeRightCameraSim"/>
+    </RobotNode>
+
+    <RobotNode name="EyeRightCamera">
+        <Transform>
+            <RollPitchYaw roll="0" pitch="0" yaw ="90" units="degree"/>
+        </Transform>
+        <!--Sensor type="Camera" name="RightEyeCam">
+        </Sensor-->
+    </RobotNode>
+
+    <RobotNode name="EyeRightCameraSim">
+        <Transform>
+            <RollPitchYaw roll="0" pitch="0" yaw ="180" units="degree"/>
+        </Transform>
+    </RobotNode>
+
+  <RobotNode name="Lid Right Bottom">
+    <Joint type="revolute">
+      <Axis x="0" y="1" z="0"/>
+        <MaxVelocity unit="radian" value="1"/>
+        <MaxAcceleration value="10"/>
+        <MaxTorque value="3000"/>
+    </Joint>
+    <Visualization>
+      <File type="Inventor">fullmodel/lid_r_bottom.wrl</File>
+    </Visualization>
+  </RobotNode>
+
+  <RobotNode name="Lid Right Top">
+  <Joint type="revolute">
+    <Axis x="0" y="1" z="0"/>
+      <MaxVelocity unit="radian" value="1"/>
+      <MaxAcceleration value="10"/>
+      <MaxTorque value="3000"/>
+  </Joint>
+    <Visualization>
+      <File type="Inventor">fullmodel/lid_r_top.wrl</File>
+    </Visualization>
+  </RobotNode>
+  
+    <RobotNodeSet name="Head" kinematicRoot="Neck_1_Pitch">
+        <Node name="Neck_1_Pitch"/>
+        <Node name="Neck_2_Roll"/>
+        <Node name="Neck_3_Yaw"/>
+        <Node name="Head_Tilt"/>
+        <Node name="Cameras"/>
+        <Node name="Eye_Left"/>
+        <Node name="Eye_Right"/>
+    </RobotNodeSet>
+
+
+    <RobotNodeSet name="Neck" kinematicRoot="Neck_1_Pitch">
+        <Node name="Neck_1_Pitch"/>
+        <Node name="Neck_2_Roll"/>
+        <Node name="Neck_3_Yaw"/>
+        <Node name="Head_Tilt"/>
+    </RobotNodeSet>
+
+
+
+
+    <RobotNodeSet name="IKVirtualGaze" kinematicRoot="Neck_1_Pitch" tcp="VirtualCentralGaze">
+        <Node name="Neck_1_Pitch"/>
+        <Node name="Neck_2_Roll"/>
+        <Node name="Neck_3_Yaw"/>
+        <Node name="Cameras"/>
+        <Node name="VirtualCentralGaze"/>
+    </RobotNodeSet>
+
+
+
+
+    <RobotNodeSet name="IKVirtualGazeNoEyes" kinematicRoot="Neck_1_Pitch" tcp="VirtualCentralGaze">
+        <Node name="Neck_1_Pitch"/>
+        <Node name="Neck_2_Roll"/>
+        <Node name="Neck_3_Yaw"/>
+        <Node name="VirtualCentralGaze"/>
+    </RobotNodeSet>
+
+
+
+    <RobotNodeSet name="IKVirtualGaze_NoRoll" kinematicRoot="Neck_1_Pitch" tcp="VirtualCentralGaze">
+        <Node name="Neck_1_Pitch"/>
+        <Node name="Neck_3_Yaw"/>
+        <Node name="Cameras"/>
+        <Node name="VirtualCentralGaze"/>
+    </RobotNodeSet>
+
+
+    <RobotNodeSet name="IKVirtualGazeKinect" kinematicRoot="Neck_1_Pitch" tcp="Kinect">
+        <Node name="Neck_1_Pitch"/>
+        <Node name="Neck_2_Roll"/>
+        <Node name="Neck_3_Yaw"/>
+        <Node name="Kinect"/>
+    </RobotNodeSet>
+
+
+    <RobotNodeSet name="IKVirtualGaze_WithHeadTilt" kinematicRoot="Neck_1_Pitch" tcp="VirtualCentralGaze">
+        <Node name="Neck_1_Pitch"/>
+        <Node name="Neck_2_Roll"/>
+        <Node name="Neck_3_Yaw"/>
+        <Node name="Head_Tilt"/>
+        <Node name="Cameras"/>
+        <Node name="VirtualCentralGaze"/>
+    </RobotNodeSet>
+
+</Robot>