Primo Commit

roboglue_ros_ws.workspace

@@ -0,0 +1,8 @@
+<?xml version="1.0"?>
+<Workspace>
+    <Distribution name="kinetic"/>
+    <DefaultBuildSystem value="0"/>
+    <WatchDirectories>
+        <Directory>src</Directory>
+    </WatchDirectories>
+</Workspace>

src/CMakeLists.txt

@@ -0,0 +1 @@
+/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake

src/bio_ik-kinetic-support/CMakeLists.txt

@@ -0,0 +1,129 @@
+cmake_minimum_required(VERSION 2.8.12)
+project(bio_ik)
+
+if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE)
+  message("${PROJECT_NAME}: You did not request a specific build type: Choosing 'Release' for maximum performance")
+  set(CMAKE_BUILD_TYPE Release)
+endif()
+
+find_package(catkin REQUIRED COMPONENTS
+  moveit_core
+  moveit_ros_planning
+  pluginlib
+  roscpp
+  tf
+  tf_conversions
+  eigen_conversions
+)
+
+find_package(OpenMP)
+# the specific flag is not yet present in cmake 2.8.12
+if(OpenMP_CXX_FOUND OR OPENMP_FOUND)
+  message(STATUS "OPENMP FOUND")
+  add_compile_options(${OpenMP_CXX_FLAGS})
+  if(NOT OpenMP_CXX_LIBRARIES)
+    # cmake 2.8.12 does not yet specify the library - assume we might need libgomp
+    set(OpenMP_LIBS gomp)
+  else()
+    set(OpenMP_LIBS ${OpenMP_CXX_LIBRARIES})
+  endif()
+else()
+    message(WARNING "OPENMP NOT FOUND. You will suffer performance loss.")
+    set(OpenMP_LIBS)
+endif()
+
+option(USE_FANN "build the neural-network-based IK solver (experimental)" OFF)
+if(USE_FANN)
+    find_library(FANN_LIBRARIES NAMES fann)
+    find_path(FANN_INCLUDE_DIRS NAMES fann.h)
+    if(NOT FANN_INCLUDE_DIRS OR NOT FANN_LIBRARIES)
+        message(FATAL_ERROR "Neural network solver requested, but libfann was not found.")
+    else()
+        message("Found libfann: ${FANN_LIBRARIES} / ${FANN_INCLUDE_DIRS}")
+    endif()
+else()
+    set(FANN_LIBRARIES)
+    set(FANN_INCLUDE_DIRS)
+endif()
+
+option(USE_CPPOPTLIB "Include gradient-based solvers from CppNumericalSolvers (bio_ik also provides its own solver)" OFF)
+if(USE_CPPOPTLIB)
+    find_path(CPPOPTLIB_INCLUDE_DIRS
+        NAMES cppoptlib/solver/bfgssolver.h
+        HINTS ../../CppNumericalSolvers/include)
+    if(NOT CPPOPTLIB_INCLUDE_DIRS)
+        message(FATAL_ERROR "cppoptlib support requested, but the headers could not be found.")
+    else()
+        message("Found cppoptlib: ${CPPOPTLIB_INCLUDE_DIRS}")
+    endif()
+    add_definitions(-DENABLE_CPP_OPTLIB)
+else()
+    set(CPPOPTLIB_INCLUDE_DIRS)
+endif()
+
+catkin_package(
+    INCLUDE_DIRS include
+    LIBRARIES ${PROJECT_NAME}
+    CATKIN_DEPENDS
+        moveit_core
+        moveit_ros_planning
+        pluginlib
+        roscpp
+        tf
+        tf_conversions
+)
+
+add_compile_options(-frecord-gcc-switches)
+
+add_compile_options(-std=c++11)
+
+add_compile_options($<$<CONFIG:Release>:-O3>)
+add_compile_options($<$<CONFIG:Release>:-ftree-vectorize>)
+add_compile_options($<$<CONFIG:Release>:-ffast-math>)
+
+include_directories(
+    include
+    ${FANN_INCLUDE_DIRS}
+    ${CPPOPTLIB_INCLUDE_DIRS}
+    ${catkin_INCLUDE_DIRS}
+)
+
+set(SOURCES
+    src/goal_types.cpp
+    src/kinematics_plugin.cpp
+    src/problem.cpp
+
+    src/ik_test.cpp
+    src/ik_gradient.cpp
+    src/ik_evolution_1.cpp
+    src/ik_evolution_2.cpp
+)
+
+if(USE_FANN)
+    list(APPEND SOURCES src/ik_neural.cpp)
+endif()
+
+if(USE_CPPOPTLIB)
+    list(APPEND SOURCES src/ik_cppoptlib.cpp)
+endif()
+
+add_library(${PROJECT_NAME} ${SOURCES})
+
+set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION})
+
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+target_link_libraries(${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+  ${FANN_LIBRARIES}
+  ${OpenMP_LIBS}
+
+  -static-libgcc
+  -static-libstdc++
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+
+install(TARGETS ${PROJECT_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
+
+install(FILES bio_ik_kinematics_description.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

src/bio_ik-kinetic-support/README.md

@@ -0,0 +1,393 @@
+# bio-ik
+
+## Installation and Setup
+
+You will need ROS version Indigo or newer (wiki.ros.org).
+The software was developed on Ubuntu Linux 16.04 LTS with ROS Kinetic,
+but has also been tested on Ubuntu Linux 14.04 LTS with ROS Indigo.
+Newer versions of ROS should work, but may need some adaptation.
+See below for version specific instructions.
+
+* Download the `bio_ik` package and unpack into your catkin workspace.
+* Run `catkin_make` to compile your workspace:
+  ```
+    roscd
+    cd src
+    git clone https://github.com/TAMS-Group/bio_ik.git
+    roscd
+    catkin_make
+  ```
+
+* Configure Moveit to use bio-ik as the kinematics solver (see next section).
+* Use Moveit to plan and execute motions or use your own code
+  together with `move_group` node to move your robot.
+* As usual, the public API is specified in the public header files for the `bio_ik` package,
+  located in the `include/bio_ik` subdirectory; 
+  the sources including a few private header files are in the `src` subdirectory.
+
+
+## Basic Usage
+
+For ease of use and compatibility with existing code, 
+the bio-ik algorithm is encapsulated as a Moveit kinematics plugin.
+Therefore, bio-ik can be used as a direct replacement of
+the default Orocos/KDL-based IK solver.
+Given the name of an end-effector and a 6-DOF target pose,
+bio-ik will search a valid robot joint configuration that reaches the given target.
+
+In our tests (see below), both in terms of success rate and solution time,
+bio-ik regularly outperformed the Orocos [1] solver
+and is competitive with trac-ik [2].
+The bio-ik algorithm can also be used for high-DOF system like robot snakes,
+and it will automatically converge to the best approximate solutions
+for low-DOF arms where some target poses are not reachable exactly.
+
+While you can write the Moveit configuration files by hand, 
+the easiest way is to run the Moveit setup assistant for your robot, 
+and then to select bio-ik as the IK solver when configuring the end effectors.
+Once configured, the solver can be called using the standard Moveit API
+or used interactively from rviz using the MotionPlanning GUI plugin.
+
+* Make sure that you have a URDF (or xacro) model for your robot.
+* Run the moveit setup assistant to create the Moveit configuration files:
+
+  ```
+  rosrun moveit_setup_assistant moveit_setup_assistant
+
+  ```
+*  The setup assistant automatically searches for all available IK solver plugins
+  in your workspace. 
+  Therefore, you can just select select bio-ik as the IK solver 
+  from the drop-down list for every end effector and then configure 
+  the kinematics parameters, namely the default position accuracy (meters)
+  and the timeout (in seconds). For typical 6-DOF or 7-DOF arms,
+  an accuracy of 0.001 m (or smaller) and a timeout of 1 msec should be ok.
+  More complex robots might need a longer timeout.
+* Generate the moveit configuration files from the setup assistant.
+  Of course, you can also edit the `config/kinematics.yaml` configuration
+  file with your favorite text editor. 
+  For example, a configuration for the PR2 robot might look like this:
+
+  ```
+    # example kinematics.yaml for the PR2 robot
+    right_arm:
+      # kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+      # kinematics_solver_attempts: 1
+      kinematics_solver: bio_ik/BioIKKinematicsPlugin
+      kinematics_solver_search_resolution: 0.005
+      kinematics_solver_timeout: 0.005
+      kinematics_solver_attempts: 1
+    left_arm:
+      kinematics_solver: bio_ik/BioIKKinematicsPlugin
+      kinematics_solver_search_resolution: 0.005
+      kinematics_solver_timeout: 0.005
+      kinematics_solver_attempts: 1
+    all:
+      kinematics_solver: bio_ik/BioIKKinematicsPlugin
+      kinematics_solver_search_resolution: 0.005
+      kinematics_solver_timeout: 0.02
+      kinematics_solver_attempts: 1
+
+    # optional bio-ik configuration parameters
+    #  center_joints_weight: 1
+    #  minimal_displacement_weight: 1
+    #  avoid_joint_limits_weight: 1
+  ```
+
+
+* For a first test, run the Moveit-created demo launch. Once rviz is running,
+  enable the motion planning plugin, then select one of the end effectors
+  of you robot. Rviz should show an 6-D (position and orientation)
+  interactive marker for the selected end-effector(s).
+  Move the interactive marker and watch bio-ik calculating poses for your robot.
+
+  If you also installed the bio-ik demo (see below), you should be able
+  to run one of the predefined demos:
+  ```
+    roslaunch pr2_bioik_moveit demo.launch
+    roslaunch pr2_bioik_moveit valve.launch
+    roslaunch pr2_bioik_moveit dance.launch
+  ```
+
+* You are now ready to use bio-ik from your C/C++ and Python programs,
+  using the standard Moveit API.
+  To explicitly request an IK solution in C++:
+  ```
+    robot_model_loader::RobotModelLoader robot_model_loader(robot);
+
+    auto robot_model = robot_model_loader.getModel();
+    auto joint_model_group = robot_model->getJointModelGroup(group);
+    auto tip_names = joint_model_group->getSolverInstance()->getTipFrames();
+
+    kinematics::KinematicsQueryOptions opts;
+    opts.return_approximate_solution = true; // optional
+
+    robot_state::RobotState robot_state_ik(robot_model);
+
+    // traditional "basic" bio-ik usage. The end-effector goal poses
+    // and end-effector link names are passed into the setFromIK()
+    // call. The KinematicsQueryOptions are empty.
+    // 
+    bool ok = robot_state_ik.setFromIK(
+                joint_model_group, // joints to be used for IK
+                tip_transforms,    // multiple end-effector goal poses
+                tip_names,         // names of the end-effector links
+                attempts, timeout, // solver attempts and timeout
+                moveit::core::GroupStateValidityCallbackFn(),
+                opts               // mostly empty
+              );
+  ```
+
+## Advanced Usage
+
+For many robot applications, it is essential to specify more than just
+a single end-effector pose. Typical examples include
+
+* two-arm manipulation tasks on two-arm robots (e.g. Baxter)
+* multi end-effector tasks with shared kinematic links
+* grasping and manipulation tasks with multi-finger hands
+* full-body motion on humanoid robots
+* reaching tasks with additional constraints (e.g. shoulder position)
+* incremental tool motions without robot arm configuration changes
+* and many more
+
+In bio-ik, such tasks are specified as a combination of multiple
+individual *goals*.  
+The algorithm then tries to find a robot configuration
+that fulfills all given goals simultaneously by minimizing 
+a quadratic error function built from the weighted individual goals.
+While the current Moveit API does not support multiple-goals tasks directly,
+it provides the KinematicQueryOptions class.
+Therefore, bio-ik simply provides a set of predefined motion goals,
+and a combination of the user-specified goals is passed via Moveit to the IK solver.
+No API changes are required in Moveit, but using the IK solver now consists
+passing the weighted goals via the KinematicQueryOptions.
+The predefined goals include:
+
+* *PoseGoal*: a full 6-DOF robot pose
+* *PositionGoal*: a 3-DOF (x,y,z) position
+* *OrientationGoal*: a 3-DOF orientation, encoded as a quaternion (qx,qy,qz,qw)
+* *LookAtGoal*: a 3-DOF (x,y,z) position intended as a looking direction
+   for a camera or robot head
+* *JointGoal*: a set of joint angles, e.g. to specify a 
+* *FunctionGoal*: an arbitrary function of the robot joint values,
+   e.g. to model underactuated joints or mimic joints
+* and several more
+
+
+
+To solve a motion problem on your robot, the trick now is to construct
+a suitable combination of individual goals. 
+
+![PR2 turning a valve](doc/pr2_vt_0.png)
+
+In the following example, we want to grasp and then _slowly turn 
+a valve wheel_ with the left and right gripers of the PR2 robot:
+
+  ```
+    bio_ik::BioIKKinematicsQueryOptions ik_options;
+    ik_options.replace = true;
+    ik_options.return_approximate_solution = true;
+
+    auto* ll_goal = new bio_ik::PoseGoal();
+    auto* lr_goal = new bio_ik::PoseGoal();
+    auto* rl_goal = new bio_ik::PoseGoal();
+    auto* rr_goal = new bio_ik::PoseGoal();
+    ll_goal->setLinkName("l_gripper_l_finger_tip_link");
+    lr_goal->setLinkName("l_gripper_r_finger_tip_link");
+    rl_goal->setLinkName("r_gripper_l_finger_tip_link");
+    rr_goal->setLinkName("r_gripper_r_finger_tip_link");
+    ik_options.goals.emplace_back(ll_goal);
+    ik_options.goals.emplace_back(lr_goal);
+    ik_options.goals.emplace_back(rl_goal);
+    ik_options.goals.emplace_back(rr_goal);
+  ```
+
+We also set a couple of secondary goals. 
+First, we want that the head of the PR2 looks at the center of the valve.
+Second, we want to avoid joint-limits on all joints, if possible.
+Third, we want that IK solutions are as close as possible to the previous
+joint configuration, meaning small and efficient motions. This is handled
+by adding the MinimalDisplacementGoal.
+Fourth, we want to avoid torso lift motions, which are very slow on the PR2.
+All of this is specified easily:
+
+ ```
+    auto* lookat_goal = new bio_ik::LookAtGoal();
+    lookat_goal->setLinkName("sensor_mount_link");
+    ik_options.goals.emplace_back(lookat_goal);
+
+    auto* avoid_joint_limits_goal = new bio_ik::AvoidJointLimitsGoal();
+    ik_options.goals.emplace_back(avoid_joint_limits_goal);
+
+    auto* minimal_displacement_goal = new bio_ik::MinimalDisplacementGoal();
+    ik_options.goals.emplace_back(minimal_displacement_goal);
+
+    auto* torso_goal = new bio_ik::PositionGoal();
+    torso_goal->setLinkName("torso_lift_link");
+    torso_goal->setWeight(1);
+    torso_goal->setPosition(tf::Vector3( -0.05, 0, 1.0 ));
+    ik_options.goals.emplace_back(torso_goal);
+  ```
+
+For the actual turning motion, we calculate a set of required gripper
+poses in a loop:
+  ```
+    for(int i = 0; ; i++) {
+        tf::Vector3 center(0.7, 0, 1);
+
+        double t = i * 0.1;
+        double r = 0.1;
+        double a = sin(t) * 1;
+        double dx = fmin(0.0, cos(t) * -0.1);
+        double dy = cos(a) * r;
+        double dz = sin(a) * r;
+
+        tf::Vector3 dl(dx, +dy, +dz);
+        tf::Vector3 dr(dx, -dy, -dz);
+        tf::Vector3 dg = tf::Vector3(0, cos(a), sin(a)) * (0.025 + fmin(0.025, fmax(0.0, cos(t) * 0.1)));
+
+        ll_goal->setPosition(center + dl + dg);
+        lr_goal->setPosition(center + dl - dg);
+        rl_goal->setPosition(center + dr + dg);
+        rr_goal->setPosition(center + dr - dg);
+
+        double ro = 0;
+        ll_goal->setOrientation(tf::Quaternion(tf::Vector3(1, 0, 0), a + ro));
+        lr_goal->setOrientation(tf::Quaternion(tf::Vector3(1, 0, 0), a + ro));
+        rl_goal->setOrientation(tf::Quaternion(tf::Vector3(1, 0, 0), a + ro));
+        rr_goal->setOrientation(tf::Quaternion(tf::Vector3(1, 0, 0), a + ro));
+
+        lookat_goal->setAxis(tf::Vector3(1, 0, 0));
+        lookat_goal->setTarget(rr_goal->getPosition());
+
+        // "advanced" bio-ik usage. The call parameters for the end-effector
+        // poses and end-effector link names are left empty; instead the
+        // requested goals and weights are passed via the ik_options object.
+        // 
+        robot_state.setFromIK(
+                      joint_model_group,           // active PR2 joints
+                      EigenSTL::vector_Affine3d(), // no explicit poses here
+                      std::vector<std::string>(),  // no end effector links here
+                      0, 0.0,                      // take values from YAML file
+                      moveit::core::GroupStateValidityCallbackFn(), 
+                      ik_options       // four gripper goals and secondary goals
+                    );
+
+        ... // check solution validity and actually move the robot
+    }
+  ```
+
+When you execute the code, the PR2 will reach for the valve wheel
+and turn it. Every once in a while it can't reach the valve with
+its current arm configuration and will regrasp the wheel.
+
+
+## How it works
+
+The bio-ik solver is based on a memetic algorithm that combines 
+traditional gradient-based search with a hybrid genetic
+and particle-swarm optimization [3].
+See [4] for the basic idea and the details of the evolutionary operators 
+and [5] for the description of the algorithm applied to many IK and manipulation tasks.
+
+Internally, vectors of all robot joint values are used to encode
+different intermediate solutions (the *genotype* of the genetic algorithm).
+During the optimization, joint values are always checked against the
+active lower and upper joint limits, so that only valid robot configurations
+are generated.
+
+To calculate the fitness of individuals, the cumulative error over all
+given individual goals is calculated. 
+Any individual with zero error is an exact solution for the IK problem, 
+while individuals with small error correspond to approximate solutions.
+
+Individuals are sorted by their fitness, and gradient-based optimization
+is tried on the best few configuration, resulting in fast convergence 
+and good performance for many problems. 
+If no solution is found from the gradient-based optimization,
+new individuals are created by a set of mutation and recombination operators,
+resulting in good search-space exploration.
+
+
+
+## Example Performance data
+
+To be written.
+
+## Running the Self-Tests
+
+We have tested bio-ik on many different robot arms,
+both using the tranditional single end-effector API
+and the advanced multi end-effector API based on the KinematicsQueryOptions.
+
+One simple selftest consists of generating random valid robot configurations, 
+running forward kinematics to calculate the resulting end-effector pose,
+and the querying the IK plugin to find a suitable robot joint configuration.
+Success is then checked by running forrward kinematics again and checking
+that the end-effector pose for the generated IK solution matches the target pose.
+This approach can be run easily for thousands or millions of random poses,
+samples the full workspace of the robot,
+and allows to quickly generate success-rate and solution-time estimates
+for the selected IK solver.
+
+Of course, running the tests requires installing the corresponding robot
+models and adds a lot of dependencies. 
+Therefore, those tests are not included in the standard bio-ik package,
+but are packaged separately.
+
+For convenience, we provide the `pr2_bioik_moveit` package,
+which also includes a few bio-ik demos for the PR2 service robot.
+These are kinematics only demos; but of course you can also try
+running the demos on the real robot (if you have one) or the Gazebo
+simulator (if you installed Gazebo).
+
+Simply clone the PR2 description package (inside `pr2_common`)
+and the `pr2_bioik_moveit` package into your catkin workspace:
+  ```
+    roscd
+    cd src
+    git clone https://github.com/PR2/pr2_common.git
+    git clone https://github.com/TAMS/pr2_bioik_moveit.git
+    catkin_make
+  ```
+
+For the FK-IK-FK performance test, please run
+
+  ```
+  roslaunch pr2_bioik_moveit env_pr2.launch
+  roslaunch pr2_bioik_moveit test_fk_ik.launch
+  ... // wait for test completion and results summary
+  ```
+
+
+
+
+
+## References
+
+ 1. Orocos Kinematics and Dynamics, http://www.orocos.org
+ 
+ 2. P. Beeson and B. Ames, *TRAC-IK: 
+    An open-source library for improved solving of generic inverse kinematics*,
+    Proceedings of the IEEE RAS Humanoids Conference, Seoul, Korea, November 2015.
+
+ 3. Philipp Ruppel, Prformance optimization and implementation 
+    of evolutionary inverse kinematics in ROS*,
+    MSc thesis, University of Hamburg, 2017
+    [PDF](https://tams.informatik.uni-hamburg.de/publications/2017/MSc_Philipp_Ruppel.pdf)
+
+
+ 4. Sebastian Starke, Norman Hendrich, Jianwei Zhang,  *A Memetic 
+    Evolutionary Algorithm for Real-Time Articulated Kinematic Motion*, 
+    IEEE Intl. Congress on Evolutionary Computation (CEC-2017), p.2437-2479, June 4-8, 2017, 
+    San Sebastian, Spain. 
+    DOI: [10.1109/CEC.2017.7969605](http://doi.org/10.1109/CEC.2017.7969605)
+
+ 5. Sebastian Starke, Norman Hendrich, Dennis Krupke, Jianwei Zhang, *Multi-Objective 
+    Evolutionary Optimisation for Inverse Kinematics 
+    on Highly Articulated and Humanoid Robots*, 
+    IEEE Intl. Conference on Intelligent Robots and Systems (IROS-2017), 
+    September 24-28, 2017, Vancouver, Canada 
+
+

src/bio_ik-kinetic-support/bio_ik_kinematics_description.xml

@@ -0,0 +1,4 @@
+<library path="lib/libbio_ik">
+    <class name="bio_ik/BioIKKinematicsPlugin" type="bio_ik_kinematics_plugin::BioIKKinematicsPlugin" base_class_type="kinematics::KinematicsBase">
+    </class>
+</library>

src/bio_ik-kinetic-support/include/bio_ik/bio_ik.h

@@ -0,0 +1,50 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016-2017, Philipp Sebastian Ruppel
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#pragma once
+
+#include <functional>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <tf/tf.h>
+
+#include "goal.h"
+#include "goal_types.h"
+
+namespace bio_ik
+{
+
+}

src/bio_ik-kinetic-support/include/bio_ik/frame.h

@@ -0,0 +1,258 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016-2017, Philipp Sebastian Ruppel
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#pragma once
+
+#include <vector>
+
+#include <Eigen/Dense>
+#include <tf_conversions/tf_kdl.h>
+
+namespace bio_ik
+{
+
+typedef tf::Quaternion Quaternion;
+typedef tf::Vector3 Vector3;
+
+struct Frame
+{
+    Vector3 pos;
+    double __padding[4 - (sizeof(Vector3) / sizeof(double))];
+    Quaternion rot;
+    inline Frame() {}
+    inline Frame(const tf::Vector3& pos, const tf::Quaternion& rot)
+        : pos(pos)
+        , rot(rot)
+    {
+    }
+    explicit inline Frame(const KDL::Frame& kdl)
+    {
+        pos = tf::Vector3(kdl.p.x(), kdl.p.y(), kdl.p.z());
+        double qx, qy, qz, qw;
+        kdl.M.GetQuaternion(qx, qy, qz, qw);
+        rot = tf::Quaternion(qx, qy, qz, qw);
+    }
+    explicit inline Frame(const geometry_msgs::Pose& msg)
+    {
+        tf::quaternionMsgToTF(msg.orientation, rot);
+        pos = tf::Vector3(msg.position.x, msg.position.y, msg.position.z);
+    }
+    explicit inline Frame(const Eigen::Affine3d& f)
+    {
+        pos = tf::Vector3(f.translation().x(), f.translation().y(), f.translation().z());
+        Eigen::Quaterniond q(f.rotation());
+        rot = tf::Quaternion(q.x(), q.y(), q.z(), q.w());
+    }
+
+    inline const Vector3& getPosition() const { return pos; }
+    inline const Quaternion& getOrientation() const { return rot; }
+    inline void setPosition(const Vector3& p) { pos = p; }
+    inline void setOrientation(const Quaternion& q) { rot = q; }
+
+private:
+    template <size_t i> struct IdentityFrameTemplate
+    {
+        static const Frame identity_frame;
+    };
+
+public:
+    static inline const Frame& identity() { return IdentityFrameTemplate<0>::identity_frame; }
+};
+
+inline void frameToKDL(const Frame& frame, KDL::Frame& kdl_frame)
+{
+    KDL::Rotation kdl_rot;
+    KDL::Vector kdl_pos;
+    tf::quaternionTFToKDL(frame.rot, kdl_rot);
+    tf::vectorTFToKDL(frame.pos, kdl_pos);
+    kdl_frame = KDL::Frame(kdl_rot, kdl_pos);
+}
+
+template <size_t i> const Frame Frame::IdentityFrameTemplate<i>::identity_frame(Vector3(0, 0, 0), Quaternion(0, 0, 0, 1));
+
+static std::ostream& operator<<(std::ostream& os, const Frame& f) { return os << "(" << f.pos.x() << "," << f.pos.y() << "," << f.pos.z() << ";" << f.rot.x() << "," << f.rot.y() << "," << f.rot.z() << "," << f.rot.w() << ")"; }
+
+__attribute__((always_inline)) inline void quat_mul_vec(const tf::Quaternion& q, const tf::Vector3& v, tf::Vector3& r)
+{
+    double v_x = v.x();
+    double v_y = v.y();
+    double v_z = v.z();
+
+    // if(__builtin_expect(v_x == 0 && v_y == 0 && v_z == 0, 0)) { r = tf::Vector3(0, 0, 0); return; }
+    // if(v_x == 0 && v_y == 0 && v_z == 0) { r = tf::Vector3(0, 0, 0); return; }
+
+    double q_x = q.x();
+    double q_y = q.y();
+    double q_z = q.z();
+    double q_w = q.w();
+
+    if((v_x == 0 && v_y == 0 && v_z == 0) || (q_x == 0 && q_y == 0 && q_z == 0 && q_w == 1))
+    {
+        r = v;
+        return;
+    }
+    // if((v_x + v_y + v_z == 0 && v_x == 0 && v_y == 0) || (q_x + q_y + q_z == 0 && q_x == 0 && q_y == 0 && q_w == 1)) { r = v; return; }
+    // if(q_x == 0 && q_y == 0 && q_z == 0 && q_w == 1) { r = v; return; }
+
+    double t_x = q_y * v_z - q_z * v_y;
+    double t_y = q_z * v_x - q_x * v_z;
+    double t_z = q_x * v_y - q_y * v_x;
+
+    double r_x = q_w * t_x + q_y * t_z - q_z * t_y;
+    double r_y = q_w * t_y + q_z * t_x - q_x * t_z;
+    double r_z = q_w * t_z + q_x * t_y - q_y * t_x;
+
+    r_x += r_x;
+    r_y += r_y;
+    r_z += r_z;
+
+    r_x += v_x;
+    r_y += v_y;
+    r_z += v_z;
+
+    r.setX(r_x);
+    r.setY(r_y);
+    r.setZ(r_z);
+}
+
+__attribute__((always_inline)) inline void quat_mul_quat(const tf::Quaternion& p, const tf::Quaternion& q, tf::Quaternion& r)
+{
+    double p_x = p.x();
+    double p_y = p.y();
+    double p_z = p.z();
+    double p_w = p.w();
+
+    double q_x = q.x();
+    double q_y = q.y();
+    double q_z = q.z();
+    double q_w = q.w();
+
+    double r_x = (p_w * q_x + p_x * q_w) + (p_y * q_z - p_z * q_y);
+    double r_y = (p_w * q_y - p_x * q_z) + (p_y * q_w + p_z * q_x);
+    double r_z = (p_w * q_z + p_x * q_y) - (p_y * q_x - p_z * q_w);
+    double r_w = (p_w * q_w - p_x * q_x) - (p_y * q_y + p_z * q_z);
+
+    r.setX(r_x);
+    r.setY(r_y);
+    r.setZ(r_z);
+    r.setW(r_w);
+}
+
+__attribute__((always_inline)) inline void concat(const Frame& a, const Frame& b, Frame& r)
+{
+    tf::Vector3 d;
+    quat_mul_vec(a.rot, b.pos, d);
+    r.pos = a.pos + d;
+    quat_mul_quat(a.rot, b.rot, r.rot);
+}
+
+__attribute__((always_inline)) inline void concat(const Frame& a, const Frame& b, const Frame& c, Frame& r)
+{
+    Frame tmp;
+    concat(a, b, tmp);
+    concat(tmp, c, r);
+}
+
+__attribute__((always_inline)) inline void quat_inv(const tf::Quaternion& q, tf::Quaternion& r)
+{
+    r.setX(-q.x());
+    r.setY(-q.y());
+    r.setZ(-q.z());
+    r.setW(q.w());
+}
+
+__attribute__((always_inline)) inline void invert(const Frame& a, Frame& r)
+{
+    Frame tmp;
+    quat_inv(a.rot, r.rot);
+    quat_mul_vec(r.rot, -a.pos, r.pos);
+}
+
+__attribute__((always_inline)) inline void change(const Frame& a, const Frame& b, const Frame& c, Frame& r)
+{
+    Frame tmp;
+    invert(b, tmp);
+    concat(a, tmp, c, r);
+}
+
+__attribute__((always_inline)) inline Frame inverse(const Frame& f)
+{
+    Frame r;
+    invert(f, r);
+    return r;
+}
+
+__attribute__((always_inline)) inline Frame operator*(const Frame& a, const Frame& b)
+{
+    Frame r;
+    concat(a, b, r);
+    return r;
+}
+
+__attribute__((always_inline)) inline Frame& operator*=(Frame& a, const Frame& b)
+{
+    a = a * b;
+    return a;
+}
+
+__attribute__((always_inline)) inline void normalizeFast(Quaternion& q)
+{
+    double f = (3.0 - q.length2()) * 0.5;
+    q.setX(q.x() * f);
+    q.setY(q.y() * f);
+    q.setZ(q.z() * f);
+    q.setW(q.w() * f);
+}
+
+__attribute__((always_inline)) inline KDL::Twist frameTwist(const Frame& a, const Frame& b)
+{
+    auto frame = inverse(a) * b;
+    KDL::Twist t;
+    t.vel.x(frame.pos.x());
+    t.vel.y(frame.pos.y());
+    t.vel.z(frame.pos.z());
+
+    double ra = frame.rot.getAngle();
+    // double ra = frame.rot.getAngleShortestPath();
+    if(ra > +M_PI) ra -= 2 * M_PI;
+    // if(ra < -M_PI) ra += 2 * M_PI;
+
+    auto r = frame.rot.getAxis() * ra;
+    t.rot.x(r.x());
+    t.rot.y(r.y());
+    t.rot.z(r.z());
+
+    return t;
+}
+}

src/bio_ik-kinetic-support/include/bio_ik/goal.h

@@ -0,0 +1,130 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016-2017, Philipp Sebastian Ruppel
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#pragma once
+
+#include "frame.h"
+
+#include <moveit/kinematics_base/kinematics_base.h>
+
+#include <moveit/robot_model/joint_model_group.h>
+#include <moveit/robot_model/robot_model.h>
+
+namespace bio_ik
+{
+
+class RobotInfo;
+
+class GoalContext
+{
+protected:
+    const double* active_variable_positions_;
+    const Frame* tip_link_frames_;
+    std::vector<ssize_t> goal_variable_indices_;
+    std::vector<size_t> goal_link_indices_;
+    bool goal_secondary_;
+    std::vector<std::string> goal_link_names_, goal_variable_names_;
+    double goal_weight_;
+    const moveit::core::JointModelGroup* joint_model_group_;
+    std::vector<size_t> problem_active_variables_;
+    std::vector<size_t> problem_tip_link_indices_;
+    std::vector<double> initial_guess_;
+    std::vector<double> velocity_weights_;
+    const RobotInfo* robot_info_;
+    mutable std::vector<double> temp_vector_;
+
+public:
+    GoalContext() {}
+    inline const Frame& getLinkFrame(size_t i = 0) const { return tip_link_frames_[goal_link_indices_[i]]; }
+    inline const double getVariablePosition(size_t i = 0) const
+    {
+        auto j = goal_variable_indices_[i];
+        if(j >= 0)
+            return active_variable_positions_[j];
+        else
+            return initial_guess_[-1 - j];
+    }
+    inline const Frame& getProblemLinkFrame(size_t i) const { return tip_link_frames_[i]; }
+    inline size_t getProblemLinkCount() const { return problem_tip_link_indices_.size(); }
+    inline size_t getProblemLinkIndex(size_t i) const { return problem_tip_link_indices_[i]; }
+    inline double getProblemVariablePosition(size_t i) const { return active_variable_positions_[i]; }
+    inline size_t getProblemVariableCount() const { return problem_active_variables_.size(); }
+    inline size_t getProblemVariableIndex(size_t i) const { return problem_active_variables_[i]; }
+    inline double getProblemVariableInitialGuess(size_t i) const { return initial_guess_[problem_active_variables_[i]]; }
+    inline double getProblemVariableWeight(size_t i) const { return velocity_weights_[i]; }
+    inline const RobotInfo& getRobotInfo() const { return *robot_info_; }
+    void addLink(const std::string& name) { goal_link_names_.push_back(name); }
+    void addVariable(const std::string& name) { goal_variable_names_.push_back(name); }
+    void setSecondary(bool secondary) { goal_secondary_ = secondary; }
+    void setWeight(double weight) { goal_weight_ = weight; }
+    const moveit::core::JointModelGroup& getJointModelGroup() const { return *joint_model_group_; }
+    const moveit::core::RobotModel& getRobotModel() const { return joint_model_group_->getParentModel(); }
+    std::vector<double>& getTempVector() const { return temp_vector_; }
+    friend class Problem;
+};
+
+class Goal
+{
+protected:
+    bool secondary_;
+    double weight_;
+
+public:
+    Goal()
+        : weight_(1)
+        , secondary_(false)
+    {
+    }
+    virtual ~Goal() {}
+    bool isSecondary() const { return secondary_; }
+    double getWeight() const { return weight_; }
+    void setWeight(double w) { weight_ = w; }
+    virtual void describe(GoalContext& context) const
+    {
+        context.setSecondary(secondary_);
+        context.setWeight(weight_);
+    }
+    virtual double evaluate(const GoalContext& context) const { return 0; }
+};
+
+struct BioIKKinematicsQueryOptions : kinematics::KinematicsQueryOptions
+{
+    std::vector<std::unique_ptr<Goal>> goals;
+    std::vector<std::string> fixed_joints;
+    bool replace;
+    mutable double solution_fitness;
+    BioIKKinematicsQueryOptions();
+    ~BioIKKinematicsQueryOptions();
+};
+}

src/bio_ik-kinetic-support/include/bio_ik/goal_types.h

@@ -0,0 +1,661 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016-2017, Philipp Sebastian Ruppel
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#pragma once
+
+#include "goal.h"
+
+#include "robot_info.h"
+
+#include <geometric_shapes/shapes.h>
+
+#include <moveit/collision_detection/collision_robot.h>
+#include <moveit/collision_detection_fcl/collision_robot_fcl.h>
+
+#include <map>
+#include <unordered_set>
+
+#include <geometric_shapes/bodies.h>
+#include <geometric_shapes/shapes.h>
+
+namespace bio_ik
+{
+
+class LinkGoalBase : public Goal
+{
+    std::string link_name_;
+
+public:
+    LinkGoalBase()
+    {
+        weight_ = 1;
+        link_name_ = "";
+    }
+    LinkGoalBase(const std::string& link_name, double weight)
+    {
+        weight_ = weight;
+        link_name_ = link_name;
+    }
+    virtual void describe(GoalContext& context) const
+    {
+        Goal::describe(context);
+        context.addLink(link_name_);
+    }
+    void setLinkName(const std::string& link_name) { link_name_ = link_name; }
+    const std::string& getLinkName() const { return link_name_; }
+};
+
+class PositionGoal : public LinkGoalBase
+{
+    tf::Vector3 position_;
+
+public:
+    PositionGoal()
+        : position_(0, 0, 0)
+    {
+    }
+    PositionGoal(const std::string& link_name, const tf::Vector3& position, double weight = 1.0)
+        : LinkGoalBase(link_name, weight)
+        , position_(position)
+    {
+    }
+    inline const tf::Vector3& getPosition() const { return position_; }
+    inline void setPosition(const tf::Vector3& position) { position_ = position; }
+    virtual double evaluate(const GoalContext& context) const { return context.getLinkFrame().getPosition().distance2(getPosition()); }
+};
+
+class OrientationGoal : public LinkGoalBase
+{
+    tf::Quaternion orientation_;
+
+public:
+    OrientationGoal()
+        : orientation_(0, 0, 0, 1)
+    {
+    }
+    OrientationGoal(const std::string& link_name, const tf::Quaternion& orientation, double weight = 1.0)
+        : LinkGoalBase(link_name, weight)
+        , orientation_(orientation.normalized())
+    {
+    }
+    inline const tf::Quaternion& getOrientation() const { return orientation_; }
+    inline void setOrientation(const tf::Quaternion& orientation) { orientation_ = orientation.normalized(); }
+    virtual double evaluate(const GoalContext& context) const
+    {
+        // return getOrientation().distance2(context.getLinkFrame().getOrientation());
+        // return (getOrientation() - getOrientation().nearest(context.getLinkFrame().getOrientation())).length2();
+        return fmin((getOrientation() - context.getLinkFrame().getOrientation()).length2(), (getOrientation() + context.getLinkFrame().getOrientation()).length2());
+        /*return
+            (getOrientation() - context.getLinkFrame().getOrientation()).length2() *
+            (getOrientation() + context.getLinkFrame().getOrientation()).length2() * 0.5;*/
+    }
+};
+
+class PoseGoal : public LinkGoalBase
+{
+    Frame frame_;
+    double rotation_scale_;
+
+public:
+    PoseGoal()
+        : rotation_scale_(0.5)
+        , frame_(Frame::identity())
+    {
+    }
+    PoseGoal(const std::string& link_name, const tf::Vector3& position, const tf::Quaternion& orientation, double weight = 1.0)
+        : LinkGoalBase(link_name, weight)
+        , frame_(position, orientation.normalized())
+        , rotation_scale_(0.5)
+    {
+    }
+    inline const tf::Vector3& getPosition() const { return frame_.getPosition(); }
+    inline void setPosition(const tf::Vector3& position) { frame_.setPosition(position); }
+    inline const tf::Quaternion& getOrientation() const { return frame_.getOrientation(); }
+    inline void setOrientation(const tf::Quaternion& orientation) { frame_.setOrientation(orientation.normalized()); }
+    inline double getRotationScale() const { return rotation_scale_; }
+    inline void setRotationScale(double rotation_scale) { rotation_scale_ = rotation_scale; }
+    virtual double evaluate(const GoalContext& context) const
+    {
+        double e = 0.0;
+        e += context.getLinkFrame().getPosition().distance2(getPosition());
+
+        /*e +=
+            (getOrientation() - context.getLinkFrame().getOrientation()).length2() *
+            (getOrientation() + context.getLinkFrame().getOrientation()).length2() *
+            (rotation_scale_ * rotation_scale_) * 0.5;*/
+
+        /*double a = getOrientation().angleShortestPath(context.getLinkFrame().getOrientation());
+        e += a * a;
+        return e;*/
+
+        /*e += 1 - getOrientation().dot(context.getLinkFrame().getOrientation());
+        return e;*/
+
+        /*double l = getOrientation().length2() * context.getLinkFrame().getOrientation().length2();
+        //double x = _mm_rsqrt_ss(_mm_set_ss((float)l))[0];
+        double x = 1.0 / l;
+        e += (1 - getOrientation().dot(context.getLinkFrame().getOrientation()) * x) * (rotation_scale_ * rotation_scale_);
+        return e;*/
+
+        e += fmin((getOrientation() - context.getLinkFrame().getOrientation()).length2(), (getOrientation() + context.getLinkFrame().getOrientation()).length2()) * (rotation_scale_ * rotation_scale_);
+
+        // e += (1.0 - getOrientation().dot(context.getLinkFrame().getOrientation())) * (rotation_scale_ * rotation_scale_);
+
+        // e += (getOrientation() - context.getLinkFrame().getOrientation()).length2() * (rotation_scale_ * rotation_scale_);
+        // ROS_ERROR("r %f", (getOrientation() - context.getLinkFrame().getOrientation()).length2());
+        // e += (getOrientation() - getOrientation().nearest(context.getLinkFrame().getOrientation())).length2() * (rotation_scale_ * rotation_scale_);
+        return e;
+    }
+};
+
+class LookAtGoal : public LinkGoalBase
+{
+    tf::Vector3 axis_;
+    tf::Vector3 target_;
+
+public:
+    LookAtGoal()
+        : axis_(1, 0, 0)
+        , target_(0, 0, 0)
+    {
+    }
+    LookAtGoal(const std::string& link_name, const tf::Vector3& axis, const tf::Vector3& target, double weight = 1.0)
+        : LinkGoalBase(link_name, weight)
+        , axis_(axis)
+        , target_(target)
+    {
+    }
+    const tf::Vector3& getAxis() const { return axis_; }
+    const tf::Vector3& getTarget() const { return target_; }
+    void setAxis(const tf::Vector3& axis) { axis_ = axis.normalized(); }
+    void setTarget(const tf::Vector3& target) { target_ = target; }
+    virtual double evaluate(const GoalContext& context) const
+    {
+        auto& fb = context.getLinkFrame();
+        tf::Vector3 axis;
+        quat_mul_vec(fb.getOrientation(), axis_, axis);
+        return (target_ - fb.getPosition()).normalized().distance2(axis.normalized());
+        // return (target_ - axis * axis.dot(target_ - fb.getPosition())).distance2(fb.getPosition());
+    }
+};
+
+class MaxDistanceGoal : public LinkGoalBase
+{
+    tf::Vector3 target;
+    double distance;
+
+public:
+    MaxDistanceGoal()
+        : target(0, 0, 0)
+        , distance(1)
+    {
+    }
+    MaxDistanceGoal(const std::string& link_name, const tf::Vector3& target, double distance, double weight = 1.0)
+        : LinkGoalBase(link_name, weight)
+        , target(target)
+        , distance(distance)
+    {
+    }
+    const tf::Vector3& getTarget() const { return target; }
+    void setTarget(const tf::Vector3& t) { target = t; }
+    double getDistance() const { return distance; }
+    void setDistance(double d) { distance = d; }
+    virtual double evaluate(const GoalContext& context) const
+    {
+        auto& fb = context.getLinkFrame();
+        double d = fmax(0.0, fb.getPosition().distance(target) - distance);
+        return d * d;
+    }
+};
+
+class MinDistanceGoal : public LinkGoalBase
+{
+    tf::Vector3 target;
+    double distance;
+
+public:
+    MinDistanceGoal()
+        : target(0, 0, 0)
+        , distance(1)
+    {
+    }
+    MinDistanceGoal(const std::string& link_name, const tf::Vector3& target, double distance, double weight = 1.0)
+        : LinkGoalBase(link_name, weight)
+        , target(target)
+        , distance(distance)
+    {
+    }
+    const tf::Vector3& getTarget() const { return target; }
+    void setTarget(const tf::Vector3& t) { target = t; }
+    double getDistance() const { return distance; }
+    void setDistance(double d) { distance = d; }
+    virtual double evaluate(const GoalContext& context) const
+    {
+        auto& fb = context.getLinkFrame();
+        double d = fmax(0.0, distance - fb.getPosition().distance(target));
+        return d * d;
+    }
+};
+
+class LineGoal : public LinkGoalBase
+{
+    tf::Vector3 position;
+    tf::Vector3 direction;
+
+public:
+    LineGoal()
+        : position(0, 0, 0)
+        , direction(0, 0, 0)
+    {
+    }
+    LineGoal(const std::string& link_name, const tf::Vector3& position, const tf::Vector3& direction, double weight = 1.0)
+        : LinkGoalBase(link_name, weight)
+        , position(position)
+        , direction(direction.normalized())
+    {
+    }
+    const tf::Vector3& getPosition() const { return position; }
+    void setPosition(const tf::Vector3& p) { position = p; }
+    const tf::Vector3& getDirection() const { return direction; }
+    void setDirection(const tf::Vector3& d) { direction = d.normalized(); }
+    virtual double evaluate(const GoalContext& context) const
+    {
+        auto& fb = context.getLinkFrame();
+        return position.distance2(fb.getPosition() - direction * direction.dot(fb.getPosition() - position));
+    }
+};
+
+class TouchGoal : public LinkGoalBase
+{
+    tf::Vector3 position;
+    tf::Vector3 normal;
+    struct CollisionShape
+    {
+        std::vector<Vector3> vertices;
+        std::vector<fcl::Vec3f> points;
+        std::vector<int> polygons;
+        std::vector<fcl::Vec3f> plane_normals;
+        std::vector<double> plane_dis;
+        collision_detection::FCLGeometryConstPtr geometry;
+        Frame frame;
+        std::vector<std::vector<size_t>> edges;
+    };
+    struct CollisionLink
+    {
+        bool initialized;
+        std::vector<std::shared_ptr<CollisionShape>> shapes;
+        CollisionLink()
+            : initialized(false)
+        {
+        }
+    };
+    struct CollisionModel
+    {
+        std::vector<CollisionLink> collision_links;
+    };
+    mutable CollisionModel* collision_model;
+    mutable const moveit::core::LinkModel* link_model;
+
+public:
+    TouchGoal()
+        : position(0, 0, 0)
+        , normal(0, 0, 0)
+    {
+    }
+    TouchGoal(const std::string& link_name, const tf::Vector3& position, const tf::Vector3& normal, double weight = 1.0)
+        : LinkGoalBase(link_name, weight)
+        , position(position)
+        , normal(normal.normalized())
+    {
+    }
+    virtual void describe(GoalContext& context) const;
+    virtual double evaluate(const GoalContext& context) const;
+};
+
+class AvoidJointLimitsGoal : public Goal
+{
+public:
+    AvoidJointLimitsGoal(double weight = 1.0, bool secondary = true)
+    {
+        weight_ = weight;
+        secondary_ = secondary;
+    }
+    virtual double evaluate(const GoalContext& context) const
+    {
+        auto& info = context.getRobotInfo();
+        double sum = 0.0;
+        for(size_t i = 0; i < context.getProblemVariableCount(); i++)
+        {
+            size_t ivar = context.getProblemVariableIndex(i);
+            if(info.getClipMax(ivar) == DBL_MAX) continue;
+            double d = context.getProblemVariablePosition(i) - (info.getMin(ivar) + info.getMax(ivar)) * 0.5;
+            d = fmax(0.0, fabs(d) * 2.0 - info.getSpan(ivar) * 0.5);
+            d *= context.getProblemVariableWeight(i);
+            sum += d * d;
+        }
+        return sum;
+    }
+};
+
+class CenterJointsGoal : public Goal
+{
+public:
+    CenterJointsGoal(double weight = 1.0, bool secondary = true)
+    {
+        weight_ = weight;
+        secondary_ = secondary;
+    }
+    virtual double evaluate(const GoalContext& context) const
+    {
+        auto& info = context.getRobotInfo();
+        double sum = 0.0;
+        for(size_t i = 0; i < context.getProblemVariableCount(); i++)
+        {
+            size_t ivar = context.getProblemVariableIndex(i);
+            if(info.getClipMax(ivar) == DBL_MAX) continue;
+            double d = context.getProblemVariablePosition(i) - (info.getMin(ivar) + info.getMax(ivar)) * 0.5;
+            d *= context.getProblemVariableWeight(i);
+            sum += d * d;
+        }
+        return sum;
+    }
+};
+
+class MinimalDisplacementGoal : public Goal
+{
+public:
+    MinimalDisplacementGoal(double weight = 1.0, bool secondary = true)
+    {
+        weight_ = weight;
+        secondary_ = secondary;
+    }
+    virtual double evaluate(const GoalContext& context) const
+    {
+        double sum = 0.0;
+        for(size_t i = 0; i < context.getProblemVariableCount(); i++)
+        {
+            double d = context.getProblemVariablePosition(i) - context.getProblemVariableInitialGuess(i);
+            d *= context.getProblemVariableWeight(i);
+            sum += d * d;
+        }
+        return sum;
+    }
+};
+
+class JointVariableGoal : public Goal
+{
+    std::string variable_name;
+    double variable_position;
+
+public:
+    JointVariableGoal()
+        : variable_position(0)
+    {
+    }
+    JointVariableGoal(const std::string& variable_name, double variable_position, double weight = 1.0, bool secondary = false)
+        : variable_name(variable_name)
+        , variable_position(variable_position)
+    {
+        weight_ = weight;
+        secondary_ = secondary;
+    }
+    double getVariablePosition() const { return variable_position; }
+    void setVariablePosition(double p) { variable_position = p; }
+    const std::string& getVariableName() const { return variable_name; }
+    void setVariableName(const std::string& n) { variable_name = n; }
+    virtual void describe(GoalContext& context) const
+    {
+        Goal::describe(context);
+        context.addVariable(variable_name);
+    }
+    virtual double evaluate(const GoalContext& context) const
+    {
+        double d = variable_position - context.getVariablePosition();
+        return d * d;
+    }
+};
+
+class JointFunctionGoal : public Goal
+{
+    std::vector<std::string> variable_names;
+    std::function<void(std::vector<double>&)> function;
+
+public:
+    JointFunctionGoal() {}
+    JointFunctionGoal(const std::vector<std::string>& variable_names, const std::function<void(std::vector<double>&)>& function, double weight = 1.0, bool secondary = false)
+        : variable_names(variable_names)
+        , function(function)
+    {
+        weight_ = weight;
+        secondary_ = secondary;
+    }
+    void setJointVariableNames(const std::vector<std::string>& n) { variable_names = n; }
+    void setJointVariableFunction(const std::function<void(std::vector<double>&)>& f) { function = f; }
+    virtual void describe(GoalContext& context) const
+    {
+        Goal::describe(context);
+        for(auto& variable_name : variable_names)
+            context.addVariable(variable_name);
+    }
+    virtual double evaluate(const GoalContext& context) const
+    {
+        auto& temp_vector = context.getTempVector();
+        temp_vector.resize(variable_names.size());
+        for(size_t i = 0; i < variable_names.size(); i++)
+            temp_vector[i] = context.getVariablePosition(i);
+        function(temp_vector);
+        double sum = 0.0;
+        for(size_t i = 0; i < variable_names.size(); i++)
+        {
+            double d = temp_vector[i] - context.getVariablePosition(i);
+            sum += d * d;
+        }
+        return sum;
+    }
+};
+
+class BalanceGoal : public Goal
+{
+    tf::Vector3 target_, axis_;
+    struct BalanceInfo
+    {
+        tf::Vector3 center;
+        double weight;
+    };
+    mutable std::vector<BalanceInfo> balance_infos;
+
+public:
+    BalanceGoal()
+        : target_(0, 0, 0)
+        , axis_(0, 0, 1)
+    {
+    }
+    BalanceGoal(const tf::Vector3& target, double weight = 1.0)
+        : target_(target)
+        , axis_(0, 0, 1)
+    {
+        weight_ = weight;
+    }
+    const tf::Vector3& getTarget() const { return target_; }
+    const tf::Vector3& getAxis() const { return axis_; }
+    void setTarget(const tf::Vector3& target) { target_ = target; }
+    void setAxis(const tf::Vector3& axis) { axis_ = axis; }
+    virtual void describe(GoalContext& context) const;
+    virtual double evaluate(const GoalContext& context) const;
+};
+
+class LinkFunctionGoal : public LinkGoalBase
+{
+    std::function<double(const tf::Vector3&, const tf::Quaternion&)> function;
+
+public:
+    LinkFunctionGoal() {}
+    LinkFunctionGoal(const std::string& link_name, const std::function<double(const tf::Vector3&, const tf::Quaternion&)>& function, double weight = 1.0)
+        : LinkGoalBase(link_name, weight)
+        , function(function)
+    {
+    }
+    void setLinkFunction(const std::function<double(const tf::Vector3&, const tf::Quaternion&)>& f) { function = f; }
+    virtual double evaluate(const GoalContext& context) const { return function(context.getLinkFrame().getPosition(), context.getLinkFrame().getOrientation()); }
+};
+
+class SideGoal : public LinkGoalBase
+{
+    tf::Vector3 axis;
+    tf::Vector3 direction;
+
+public:
+    SideGoal()
+        : axis(0, 0, 1)
+        , direction(0, 0, 1)
+    {
+    }
+    SideGoal(const std::string& link_name, const tf::Vector3& axis, const tf::Vector3& direction, double weight = 1.0)
+        : LinkGoalBase(link_name, weight)
+        , axis(axis)
+        , direction(direction)
+    {
+    }
+    const tf::Vector3& getAxis() const { return axis; }
+    const tf::Vector3& getDirection() const { return direction; }
+    void setAxis(const tf::Vector3& a) { axis = a.normalized(); }
+    void setDirection(const tf::Vector3& d) { direction = d.normalized(); }
+    virtual double evaluate(const GoalContext& context) const
+    {
+        auto& fb = context.getLinkFrame();
+        Vector3 v;
+        quat_mul_vec(fb.getOrientation(), axis, v);
+        double f = fmax(0.0, v.dot(direction));
+        return f * f;
+    }
+};
+
+class DirectionGoal : public LinkGoalBase
+{
+    tf::Vector3 axis;
+    tf::Vector3 direction;
+
+public:
+    DirectionGoal()
+        : axis(0, 0, 1)
+        , direction(0, 0, 1)
+    {
+    }
+    DirectionGoal(const std::string& link_name, const tf::Vector3& axis, const tf::Vector3& direction, double weight = 1.0)
+        : LinkGoalBase(link_name, weight)
+        , axis(axis)
+        , direction(direction)
+    {
+    }
+    const tf::Vector3& getAxis() const { return axis; }
+    const tf::Vector3& getDirection() const { return direction; }
+    void setAxis(const tf::Vector3& a) { axis = a.normalized(); }
+    void setDirection(const tf::Vector3& d) { direction = d.normalized(); }
+    virtual double evaluate(const GoalContext& context) const
+    {
+        auto& fb = context.getLinkFrame();
+        Vector3 v;
+        quat_mul_vec(fb.getOrientation(), axis, v);
+        return v.distance2(direction);
+    }
+};
+
+class ConeGoal : public LinkGoalBase
+{
+    tf::Vector3 position;
+    double position_weight;
+    tf::Vector3 axis;
+    tf::Vector3 direction;
+    double angle;
+
+public:
+    const tf::Vector3& getPosition() const { return position; }
+    double getPositionWeight() const { return position_weight; }
+    const tf::Vector3& getAxis() const { return axis; }
+    const tf::Vector3& getDirection() const { return direction; }
+    double getAngle() const { return angle; }
+    void setPosition(const tf::Vector3& p) { position = p; }
+    void setPositionWeight(double w) { position_weight = w; }
+    void setAxis(const tf::Vector3& a) { axis = a.normalized(); }
+    void setDirection(const tf::Vector3& d) { direction = d.normalized(); }
+    void setAngle(double a) { angle = a; }
+    ConeGoal()
+        : position(0, 0, 0)
+        , position_weight(0)
+        , axis(0, 0, 1)
+        , direction(0, 0, 1)
+        , angle(0)
+    {
+    }
+    ConeGoal(const std::string& link_name, const tf::Vector3& axis, const tf::Vector3& direction, double angle, double weight = 1.0)
+        : LinkGoalBase(link_name, weight)
+        , position(0, 0, 0)
+        , position_weight(0)
+        , axis(axis)
+        , direction(direction)
+        , angle(angle)
+    {
+    }
+    ConeGoal(const std::string& link_name, const tf::Vector3& position, const tf::Vector3& axis, const tf::Vector3& direction, double angle, double weight = 1.0)
+        : LinkGoalBase(link_name, weight)
+        , position(position)
+        , position_weight(1)
+        , axis(axis)
+        , direction(direction)
+        , angle(angle)
+    {
+    }
+    ConeGoal(const std::string& link_name, const tf::Vector3& position, double position_weight, const tf::Vector3& axis, const tf::Vector3& direction, double angle, double weight = 1.0)
+        : LinkGoalBase(link_name, weight)
+        , position(position)
+        , position_weight(position_weight)
+        , axis(axis)
+        , direction(direction)
+        , angle(angle)
+    {
+    }
+    virtual double evaluate(const GoalContext& context) const
+    {
+        double sum = 0.0;
+        auto& fb = context.getLinkFrame();
+        Vector3 v;
+        quat_mul_vec(fb.getOrientation(), axis, v);
+        double d = fmax(0.0, v.angle(direction) - angle);
+        sum += d * d;
+        double w = position_weight;
+        sum += w * w * (position - fb.getPosition()).length2();
+        return sum;
+    }
+};
+}

src/bio_ik-kinetic-support/include/bio_ik/robot_info.h

@@ -0,0 +1,126 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016-2017, Philipp Sebastian Ruppel
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#pragma once
+
+#include <moveit/kinematics_base/kinematics_base.h>
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_state/robot_state.h>
+
+#include <bio_ik/goal.h>
+
+namespace bio_ik
+{
+
+class RobotInfo
+{
+    struct VariableInfo
+    {
+        double clip_min, clip_max;
+        double span;
+        double min;
+        double max;
+        double max_velocity, max_velocity_rcp;
+    };
+    std::vector<VariableInfo> variables;
+    std::vector<size_t> activeVariables;
+    std::vector<moveit::core::JointModel::JointType> variable_joint_types;
+    moveit::core::RobotModelConstPtr robot_model;
+
+    __attribute__((always_inline)) static inline double clamp2(double v, double lo, double hi)
+    {
+        if(__builtin_expect(v < lo, 0)) v = lo;
+        if(__builtin_expect(v > hi, 0)) v = hi;
+        return v;
+    }
+
+public:
+    RobotInfo() {}
+    RobotInfo(moveit::core::RobotModelConstPtr model)
+        : robot_model(model)
+    {
+        for(auto& name : model->getVariableNames())
+        {
+            auto& bounds = model->getVariableBounds(name);
+
+            VariableInfo info;
+
+            bool bounded = bounds.position_bounded_;
+
+            auto* joint_model = model->getJointOfVariable(variables.size());
+            if(auto* revolute = dynamic_cast<const moveit::core::RevoluteJointModel*>(joint_model))
+
+                if(bounds.max_position_ - bounds.min_position_ >= 2 * M_PI * 0.9999) bounded = false;
+
+            info.min = bounds.min_position_;
+            info.max = bounds.max_position_;
+
+            info.clip_min = bounded ? info.min : -DBL_MAX;
+            info.clip_max = bounded ? info.max : +DBL_MAX;
+
+            info.span = info.max - info.min;
+
+            if(!(info.span >= 0 && info.span < FLT_MAX)) info.span = 1;
+
+            info.max_velocity = bounds.max_velocity_;
+            info.max_velocity_rcp = info.max_velocity > 0.0 ? 1.0 / info.max_velocity : 0.0;
+
+            variables.push_back(info);
+        }
+
+        for(size_t ivar = 0; ivar < model->getVariableCount(); ivar++)
+        {
+            variable_joint_types.push_back(model->getJointOfVariable(ivar)->getType());
+        }
+    }
+
+public:
+    __attribute__((always_inline)) inline double clip(double p, size_t i) const
+    {
+        auto& info = variables[i];
+        return clamp2(p, info.clip_min, info.clip_max);
+    }
+
+    inline double getSpan(size_t i) const { return variables[i].span; }
+    inline double getClipMin(size_t i) const { return variables[i].clip_min; }
+    inline double getClipMax(size_t i) const { return variables[i].clip_max; }
+    inline double getMin(size_t i) const { return variables[i].min; }
+    inline double getMax(size_t i) const { return variables[i].max; }
+
+    inline bool isRevolute(size_t variable_index) const { return variable_joint_types[variable_index] == moveit::core::JointModel::REVOLUTE; }
+    inline bool isPrismatic(size_t variable_index) const { return variable_joint_types[variable_index] == moveit::core::JointModel::PRISMATIC; }
+    inline double getMaxVelocity(size_t i) const { return variables[i].max_velocity; }
+    inline double getMaxVelocityRcp(size_t i) const { return variables[i].max_velocity_rcp; }
+};
+}

src/bio_ik-kinetic-support/package.xml

@@ -0,0 +1,47 @@
+<?xml version="1.0"?>
+<package>
+
+    <name>bio_ik</name>
+    <version>1.0.0</version>
+    <description>BioIK for ROS</description>
+    <license>BSD</license>
+
+    <author email="0ruppel@informatik.uni-hamburg.de">Philipp Sebastian Ruppel</author>
+
+    <maintainer email="0ruppel@informatik.uni-hamburg.de">Philipp Sebastian Ruppel</maintainer>
+    <maintainer email="me@v4hn.de">Michael Goerner</maintainer>
+
+
+    <buildtool_depend>catkin</buildtool_depend>
+
+    <build_depend>moveit_core</build_depend>
+    <run_depend>moveit_core</run_depend>
+
+    <build_depend>pluginlib</build_depend>
+    <run_depend>pluginlib</run_depend>
+
+    <build_depend>eigen</build_depend>
+    <run_depend>eigen</run_depend>
+
+    <build_depend>moveit_ros_planning</build_depend>
+    <run_depend>moveit_ros_planning</run_depend>
+
+    <build_depend>roscpp</build_depend>
+    <run_depend>roscpp</run_depend>
+
+    <build_depend>tf</build_depend>
+    <run_depend>tf</run_depend>
+
+    <build_depend>tf_conversions</build_depend>
+    <run_depend>tf_conversions</run_depend>
+
+    <build_depend>eigen_conversions</build_depend>
+    <run_depend>eigen_conversions</run_depend>
+
+    <test_depend>rosunit</test_depend>
+
+    <export>
+        <moveit_core plugin="${prefix}/bio_ik_kinematics_description.xml"/>
+    </export>
+
+</package>

src/bio_ik-kinetic-support/src/goal_types.cpp

@@ -0,0 +1,269 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016-2017, Philipp Sebastian Ruppel
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include <bio_ik/goal_types.h>
+
+#include <geometric_shapes/bodies.h>
+#include <geometric_shapes/shapes.h>
+
+#include <mutex>
+
+namespace bio_ik
+{
+
+void TouchGoal::describe(GoalContext& context) const
+{
+    LinkGoalBase::describe(context);
+    auto* robot_model = &context.getRobotModel();
+    {
+        static std::map<const moveit::core::RobotModel*, CollisionModel*> collision_cache;
+        if(collision_cache.find(robot_model) == collision_cache.end()) collision_cache[&context.getRobotModel()] = new CollisionModel();
+        collision_model = collision_cache[robot_model];
+        collision_model->collision_links.resize(robot_model->getLinkModelCount());
+    }
+    link_model = robot_model->getLinkModel(this->getLinkName());
+    size_t link_index = link_model->getLinkIndex();
+    auto touch_goal_normal = normal.normalized();
+    // auto fbrot = fb.rot.normalized();
+    auto& collision_link = collision_model->collision_links[link_index];
+    if(!collision_link.initialized)
+    {
+        collision_link.initialized = true;
+        collision_link.shapes.resize(link_model->getShapes().size());
+        for(size_t shape_index = 0; shape_index < link_model->getShapes().size(); shape_index++)
+        {
+            collision_link.shapes[shape_index] = std::make_shared<CollisionShape>();
+            auto& s = *collision_link.shapes[shape_index];
+            s.frame = Frame(link_model->getCollisionOriginTransforms()[shape_index]);
+            auto* shape = link_model->getShapes()[shape_index].get();
+            // LOG(link_model->getName(), shape_index, link_model->getShapes().size(), typeid(*shape).name());
+            if(auto* mesh = dynamic_cast<const shapes::Mesh*>(shape))
+            {
+                struct : bodies::ConvexMesh
+                {
+                    std::vector<fcl::Vec3f> points;
+                    std::vector<int> polygons;
+                    std::vector<fcl::Vec3f> plane_normals;
+                    std::vector<double> plane_dis;
+                    void init(const shapes::Shape* shape)
+                    {
+                        type_ = shapes::MESH;
+                        scaled_vertices_ = 0;
+                        {
+                            static std::mutex mutex;
+                            std::lock_guard<std::mutex> lock(mutex);
+                            setDimensions(shape);
+                        }
+                        for(auto& v : mesh_data_->vertices_)
+                            points.emplace_back(v.x(), v.y(), v.z());
+                        for(size_t triangle_index = 0; triangle_index < mesh_data_->triangles_.size() / 3; triangle_index++)
+                        {
+                            polygons.push_back(3);
+                            polygons.push_back(mesh_data_->triangles_[triangle_index * 3 + 0]);
+                            polygons.push_back(mesh_data_->triangles_[triangle_index * 3 + 1]);
+                            polygons.push_back(mesh_data_->triangles_[triangle_index * 3 + 2]);
+                        }
+                        for(size_t triangle_index = 0; triangle_index < mesh_data_->triangles_.size() / 3; triangle_index++)
+                        {
+                            auto plane_index = mesh_data_->plane_for_triangle_[triangle_index];
+                            auto plane = mesh_data_->planes_[plane_index];
+                            plane_normals.emplace_back(plane.x(), plane.y(), plane.z());
+                            plane_dis.push_back(plane.w());
+                        }
+                    }
+                } convex;
+                convex.init(mesh);
+                s.points = convex.points;
+                s.polygons = convex.polygons;
+                s.plane_normals = convex.plane_normals;
+                s.plane_dis = convex.plane_dis;
+
+                // auto* fcl = new fcl::Convex(s.plane_normals.data(), s.plane_dis.data(), s.plane_normals.size(), s.points.data(), s.points.size(), s.polygons.data());
+
+                // workaround for fcl::Convex initialization bug
+                auto* fcl = (fcl::Convex*)::operator new(sizeof(fcl::Convex));
+                fcl->num_points = s.points.size();
+                fcl = new(fcl) fcl::Convex(s.plane_normals.data(), s.plane_dis.data(), s.plane_normals.size(), s.points.data(), s.points.size(), s.polygons.data());
+
+                s.geometry = decltype(s.geometry)(new collision_detection::FCLGeometry(fcl, link_model, shape_index));
+                s.edges.resize(s.points.size());
+                std::vector<std::unordered_set<size_t>> edge_sets(s.points.size());
+                for(size_t edge_index = 0; edge_index < fcl->num_edges; edge_index++)
+                {
+                    auto edge = fcl->edges[edge_index];
+                    if(edge_sets[edge.first].find(edge.second) == edge_sets[edge.first].end())
+                    {
+                        edge_sets[edge.first].insert(edge.second);
+                        s.edges[edge.first].push_back(edge.second);
+                    }
+                    if(edge_sets[edge.second].find(edge.first) == edge_sets[edge.second].end())
+                    {
+                        edge_sets[edge.second].insert(edge.first);
+                        s.edges[edge.second].push_back(edge.first);
+                    }
+                }
+                for(auto& p : s.points)
+                    s.vertices.emplace_back(p[0], p[1], p[2]);
+            }
+            else
+            {
+                s.geometry = collision_detection::createCollisionGeometry(link_model->getShapes()[shape_index], link_model, shape_index);
+            }
+            // LOG("b");
+        }
+        // getchar();
+    }
+}
+
+double TouchGoal::evaluate(const GoalContext& context) const
+{
+    double dmin = DBL_MAX;
+    context.getTempVector().resize(1);
+    auto& last_collision_vertex = context.getTempVector()[0];
+    auto& fb = context.getLinkFrame();
+    size_t link_index = link_model->getLinkIndex();
+    auto& collision_link = collision_model->collision_links[link_index];
+    for(size_t shape_index = 0; shape_index < link_model->getShapes().size(); shape_index++)
+    {
+        if(!collision_link.shapes[shape_index]->geometry) continue;
+        auto* shape = link_model->getShapes()[shape_index].get();
+        // LOG(shape_index, typeid(*shape).name());
+        if(auto* mesh = dynamic_cast<const shapes::Mesh*>(shape))
+        {
+            auto& s = collision_link.shapes[shape_index];
+            double d = DBL_MAX;
+            auto goal_normal = normal;
+            quat_mul_vec(fb.rot.inverse(), goal_normal, goal_normal);
+            quat_mul_vec(s->frame.rot.inverse(), goal_normal, goal_normal);
+            /*{
+                size_t array_index = 0;
+                for(size_t vertex_index = 0; vertex_index < mesh->vertex_count; vertex_index++)
+                {
+                    double dot_x = mesh->vertices[array_index++] * goal_normal.x();
+                    double dot_y = mesh->vertices[array_index++] * goal_normal.y();
+                    double dot_z = mesh->vertices[array_index++] * goal_normal.z();
+                    double e = dot_x + dot_y + dot_z;
+                    if(e < d) d = e;
+                }
+            }*/
+            if(mesh->vertex_count > 0)
+            {
+                size_t vertex_index = last_collision_vertex;
+                double vertex_dot_normal = goal_normal.dot(s->vertices[vertex_index]);
+                // size_t loops = 0;
+                while(true)
+                {
+                    bool repeat = false;
+                    for(auto vertex_index_2 : s->edges[vertex_index])
+                    {
+                        auto vertex_dot_normal_2 = goal_normal.dot(s->vertices[vertex_index_2]);
+                        if(vertex_dot_normal_2 < vertex_dot_normal)
+                        {
+                            vertex_index = vertex_index_2;
+                            vertex_dot_normal = vertex_dot_normal_2;
+                            repeat = true;
+                            break;
+                        }
+                    }
+                    if(!repeat) break;
+                    // loops++;
+                }
+                // LOG_VAR(loops);
+                d = vertex_dot_normal;
+                last_collision_vertex = vertex_index;
+            }
+            d -= normal.dot(position - fb.pos);
+            // ROS_INFO("touch goal");
+            if(d < dmin) dmin = d;
+        }
+        else
+        {
+            double offset = 10000;
+            static fcl::Sphere shape1(offset);
+            fcl::DistanceRequest request;
+            fcl::DistanceResult result;
+            auto pos1 = position - normal * offset * 2;
+            auto* shape2 = collision_link.shapes[shape_index]->geometry->collision_geometry_.get();
+            auto frame2 = Frame(fb.pos, fb.rot.normalized()) * collision_link.shapes[shape_index]->frame;
+            double d = fcl::distance(&shape1, fcl::Transform3f(fcl::Vec3f(pos1.x(), pos1.y(), pos1.z())), shape2, fcl::Transform3f(fcl::Quaternion3f(frame2.rot.w(), frame2.rot.x(), frame2.rot.y(), frame2.rot.z()), fcl::Vec3f(frame2.pos.x(), frame2.pos.y(), frame2.pos.z())), request, result);
+            d -= offset;
+            if(d < dmin) dmin = d;
+        }
+    }
+    return dmin * dmin;
+}
+
+void BalanceGoal::describe(GoalContext& context) const
+{
+    Goal::describe(context);
+    balance_infos.clear();
+    double total = 0.0;
+    for(auto& link_name : context.getRobotModel().getLinkModelNames())
+    {
+        auto link_urdf = context.getRobotModel().getURDF()->getLink(link_name);
+        if(!link_urdf) continue;
+        if(!link_urdf->inertial) continue;
+        const auto& center_urdf = link_urdf->inertial->origin.position;
+        tf::Vector3 center(center_urdf.x, center_urdf.y, center_urdf.z);
+        double mass = link_urdf->inertial->mass;
+        if(!(mass > 0)) continue;
+        balance_infos.emplace_back();
+        balance_infos.back().center = center;
+        balance_infos.back().weight = mass;
+        total += mass;
+        context.addLink(link_name);
+    }
+    for(auto& b : balance_infos)
+    {
+        b.weight /= total;
+    }
+}
+
+double BalanceGoal::evaluate(const GoalContext& context) const
+{
+    tf::Vector3 center = tf::Vector3(0, 0, 0);
+    for(size_t i = 0; i < balance_infos.size(); i++)
+    {
+        auto& info = balance_infos[i];
+        auto& frame = context.getLinkFrame(i);
+        auto c = info.center;
+        quat_mul_vec(frame.rot, c, c);
+        c += frame.pos;
+        center += c * info.weight;
+    }
+    center -= target_;
+    center -= axis_ * axis_.dot(center);
+    return center.length2();
+}
+}

src/bio_ik-kinetic-support/src/ik_base.h

@@ -0,0 +1,214 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016-2017, Philipp Sebastian Ruppel
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#pragma once
+
+#include <bio_ik/goal.h>
+
+#include "forward_kinematics.h"
+#include "problem.h"
+#include "utils.h"
+#include <bio_ik/robot_info.h>
+
+#include <random>
+
+namespace bio_ik
+{
+
+struct Random
+{
+    // std::mt19937 rng;
+    std::minstd_rand rng;
+    // std::ranlux24 rng;
+    // std::knuth_b rng;
+    // std::default_random_engine rng;
+
+    inline double random() { return std::uniform_real_distribution<double>(0, 1)(rng); }
+
+    inline std::size_t random_index(std::size_t s) { return std::uniform_int_distribution<size_t>(0, s - 1)(rng); }
+
+    std::normal_distribution<double> normal_distribution;
+    inline double random_gauss() { return normal_distribution(rng); }
+
+    inline double random(double min, double max) { return random() * (max - min) + min; }
+
+    template <class e> inline e& random_element(std::vector<e>& l) { return l[random_index(l.size())]; }
+
+    template <class e> inline const e& random_element(const std::vector<e>& l) { return l[random_index(l.size())]; }
+
+    XORShift64 _xorshift;
+    inline size_t fast_random_index(size_t mod) { return _xorshift() % mod; }
+    template <class T> inline const T& fast_random_element(const std::vector<T>& v) { return v[fast_random_index(v.size())]; }
+
+    static const size_t random_buffer_size = 1024 * 1024 * 8;
+
+    const double* make_random_buffer()
+    {
+        static std::vector<double> buf;
+        buf.resize(random_buffer_size);
+        for(auto& r : buf)
+            r = random();
+        return buf.data();
+    }
+    const double* random_buffer;
+    size_t random_buffer_index;
+    inline double fast_random()
+    {
+        double r = random_buffer[random_buffer_index & (random_buffer_size - 1)];
+        random_buffer_index++;
+        return r;
+    }
+
+    const double* make_random_gauss_buffer()
+    {
+        // LOG("make_random_gauss_buffer");
+        static std::vector<double> buf;
+        buf.resize(random_buffer_size);
+        for(auto& r : buf)
+            r = random_gauss();
+        return buf.data();
+    }
+    const double* random_gauss_buffer;
+    size_t random_gauss_index;
+    inline double fast_random_gauss()
+    {
+        double r = random_gauss_buffer[random_gauss_index & (random_buffer_size - 1)];
+        random_gauss_index++;
+        return r;
+    }
+    inline const double* fast_random_gauss_n(size_t n)
+    {
+        size_t i = random_gauss_index;
+        random_gauss_index += n;
+        if(random_gauss_index >= random_buffer_size) i = 0, random_gauss_index = n;
+        return random_gauss_buffer + i;
+    }
+
+    Random()
+        : rng(std::random_device()())
+    {
+        random_buffer = make_random_buffer();
+        random_buffer_index = _xorshift();
+        random_gauss_buffer = make_random_gauss_buffer();
+        random_gauss_index = _xorshift();
+    }
+};
+
+struct IKBase : Random
+{
+    IKParams params;
+    RobotFK model;
+    RobotInfo modelInfo;
+    int thread_index;
+    Problem problem;
+    std::vector<Frame> null_tip_frames;
+    volatile int canceled;
+
+    virtual void step() = 0;
+
+    virtual const std::vector<double>& getSolution() const = 0;
+
+    virtual void setParams(const IKParams& p) {}
+
+    IKBase(const IKParams& p)
+        : model(p.robot_model)
+        , modelInfo(p.robot_model)
+        , params(p)
+    {
+        setParams(p);
+    }
+    virtual ~IKBase() {}
+
+    virtual void initialize(const Problem& problem)
+    {
+        this->problem = problem;
+        this->problem.initialize2();
+        model.initialize(problem.tip_link_indices);
+        // active_variables = problem.active_variables;
+        null_tip_frames.resize(problem.tip_link_indices.size());
+    }
+
+    double computeSecondaryFitnessActiveVariables(const double* active_variable_positions) { return problem.computeGoalFitness(problem.secondary_goals, null_tip_frames.data(), active_variable_positions); }
+
+    double computeSecondaryFitnessAllVariables(const std::vector<double>& variable_positions) { return computeSecondaryFitnessActiveVariables(extractActiveVariables(variable_positions)); }
+
+    double computeFitnessActiveVariables(const std::vector<Frame>& tip_frames, const double* active_variable_positions) { return problem.computeGoalFitness(problem.goals, tip_frames.data(), active_variable_positions); }
+
+    double computeFitnessActiveVariables(const aligned_vector<Frame>& tip_frames, const double* active_variable_positions) { return problem.computeGoalFitness(problem.goals, tip_frames.data(), active_variable_positions); }
+
+    double computeCombinedFitnessActiveVariables(const std::vector<Frame>& tip_frames, const double* active_variable_positions)
+    {
+        double ret = 0.0;
+        ret += problem.computeGoalFitness(problem.goals, tip_frames.data(), active_variable_positions);
+        ret += problem.computeGoalFitness(problem.secondary_goals, null_tip_frames.data(), active_variable_positions);
+        return ret;
+    }
+
+    double computeCombinedFitnessActiveVariables(const aligned_vector<Frame>& tip_frames, const double* active_variable_positions)
+    {
+        double ret = 0.0;
+        ret += problem.computeGoalFitness(problem.goals, tip_frames.data(), active_variable_positions);
+        ret += problem.computeGoalFitness(problem.secondary_goals, null_tip_frames.data(), active_variable_positions);
+        return ret;
+    }
+
+    bool checkSolutionActiveVariables(const std::vector<Frame>& tip_frames, const double* active_variable_positions) { return problem.checkSolutionActiveVariables(tip_frames, active_variable_positions); }
+
+    bool checkSolution(const std::vector<double>& variable_positions, const std::vector<Frame>& tips) { return checkSolutionActiveVariables(tips, extractActiveVariables(variable_positions)); }
+
+    std::vector<double> temp_active_variable_positions;
+
+    double* extractActiveVariables(const std::vector<double>& variable_positions)
+    {
+        temp_active_variable_positions.resize(problem.active_variables.size());
+        for(size_t i = 0; i < temp_active_variable_positions.size(); i++)
+            temp_active_variable_positions[i] = variable_positions[problem.active_variables[i]];
+        return temp_active_variable_positions.data();
+    }
+
+    double computeFitness(const std::vector<double>& variable_positions, const std::vector<Frame>& tip_frames) { return computeFitnessActiveVariables(tip_frames, extractActiveVariables(variable_positions)); }
+
+    double computeFitness(const std::vector<double>& variable_positions)
+    {
+        model.applyConfiguration(variable_positions);
+        return computeFitness(variable_positions, model.getTipFrames());
+    }
+
+    virtual size_t concurrency() const { return 1; }
+};
+
+typedef IKBase IKSolver;
+
+typedef Factory<IKSolver, const IKParams&> IKFactory;
+}

src/bio_ik-kinetic-support/src/ik_cppoptlib.cpp

@@ -0,0 +1,257 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016-2017, Philipp Sebastian Ruppel
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include "ik_base.h"
+
+#include "cppoptlib/boundedproblem.h"
+#include "cppoptlib/meta.h"
+#include "cppoptlib/problem.h"
+
+namespace bio_ik
+{
+
+/*
+struct IKOptLibProblem : cppoptlib::Problem<double>
+{
+    IKBase* ik;
+    std::vector<double> fk_values;
+    IKOptLibProblem(IKBase* ik) : ik(ik)
+    {
+    }
+    void initialize()
+    {
+        // set all variables to initial guess, including inactive ones
+        fk_values = ik->problem.initial_guess;
+    }
+    double value(const TVector& x)
+    {
+        // fill in active variables and compute fitness
+        for(size_t i = 0; i < ik->problem.active_variables.size(); i++) fk_values[ik->problem.active_variables[i]] = x[i];
+        return ik->computeFitness(fk_values);
+    }
+    bool callback(const cppoptlib::Criteria<double>& state, const TVector& x)
+    {
+        // check ik timeout
+        return ros::WallTime::now().toSec() < ik->problem.timeout;
+    }
+};
+*/
+
+// problem description for cppoptlib
+struct IKOptLibProblem : cppoptlib::BoundedProblem<double>
+{
+    IKBase* ik;
+    std::vector<double> fk_values;
+    IKOptLibProblem(IKBase* ik)
+        : cppoptlib::BoundedProblem<double>(TVector(ik->problem.active_variables.size()), TVector(ik->problem.active_variables.size()))
+        , ik(ik)
+    {
+        // init bounds
+        for(size_t i = 0; i < ik->problem.active_variables.size(); i++)
+        {
+            m_lowerBound[i] = ik->modelInfo.getMin(ik->problem.active_variables[i]);
+            m_upperBound[i] = ik->modelInfo.getMax(ik->problem.active_variables[i]);
+            // m_lowerBound[i] = fmax(m_lowerBound[i], -100);
+            // m_upperBound[i] = fmin(m_upperBound[i], 100);
+        }
+    }
+    void initialize()
+    {
+        // set all variables to initial guess, including inactive ones
+        fk_values = ik->problem.initial_guess;
+    }
+    double value(const TVector& x)
+    {
+        // fill in active variables and compute fitness
+        for(size_t i = 0; i < ik->problem.active_variables.size(); i++)
+            fk_values[ik->problem.active_variables[i]] = x[i];
+        // for(size_t i = 0; i < ik->active_variables.size(); i++) LOG(i, x[i]); LOG("");
+        // for(size_t i = 0; i < ik->active_variables.size(); i++) std::cerr << ((void*)*(uint64_t*)&x[i]) << " "; std::cerr << std::endl;
+        // size_t h = 0; for(size_t i = 0; i < ik->active_variables.size(); i++) h ^= (std::hash<double>()(x[i]) << i); LOG((void*)h);
+        return ik->computeFitness(fk_values);
+    }
+    bool callback(const cppoptlib::Criteria<double>& state, const TVector& x)
+    {
+        // check ik timeout
+        return ros::WallTime::now().toSec() < ik->problem.timeout;
+    }
+};
+
+// ik solver using cppoptlib
+template <class SOLVER, int reset_if_stuck, int threads> struct IKOptLib : IKBase
+{
+    // current solution
+    std::vector<double> solution, best_solution, temp;
+
+    // cppoptlib solver
+    std::shared_ptr<SOLVER> solver;
+
+    // cppoptlib problem description
+    IKOptLibProblem f;
+
+    // reset flag
+    bool reset;
+
+    // stop criteria
+    cppoptlib::Criteria<double> crit;
+
+    IKOptLib(const IKParams& p)
+        : IKBase(p)
+        , f(this)
+    {
+    }
+
+    void initialize(const Problem& problem)
+    {
+        IKBase::initialize(problem);
+
+        // set initial guess
+        solution = problem.initial_guess;
+
+        // randomize if more than 1 thread
+        reset = false;
+        if(thread_index > 0) reset = true;
+
+        // init best solution
+        best_solution = solution;
+
+        // initialize cppoptlib problem description
+        f = IKOptLibProblem(this);
+        f.initialize();
+
+        // init stop criteria (timeout will be handled explicitly)
+        crit = cppoptlib::Criteria<double>::defaults();
+        // crit.iterations = SIZE_MAX;
+        crit.gradNorm = 1e-10;
+        // p.node_handle.param("optlib_stop", crit.gradNorm, crit.gradNorm);
+
+        if(!solver) solver = std::make_shared<SOLVER>();
+    }
+
+    const std::vector<double>& getSolution() const { return best_solution; }
+
+    void step()
+    {
+        // set stop criteria
+        solver->setStopCriteria(crit);
+
+        // random reset if stuck (and if random resets are enabled)
+        if(reset)
+        {
+            // LOG("RESET");
+            reset = false;
+            for(auto& vi : problem.active_variables)
+                solution[vi] = random(modelInfo.getMin(vi), modelInfo.getMax(vi));
+        }
+
+        // set to current positions
+        temp = solution;
+        typename SOLVER::TVector x(problem.active_variables.size());
+        for(size_t i = 0; i < problem.active_variables.size(); i++)
+            x[i] = temp[problem.active_variables[i]];
+
+        // solve
+        solver->minimize(f, x);
+
+        // get results
+        for(size_t i = 0; i < problem.active_variables.size(); i++)
+            temp[problem.active_variables[i]] = x[i];
+
+        // update solution
+        if(computeFitness(temp) < computeFitness(solution))
+        {
+            solution = temp;
+        }
+        else
+        {
+            if(reset_if_stuck) reset = true;
+        }
+
+        // update best solution
+        if(computeFitness(solution) < computeFitness(best_solution))
+        {
+            best_solution = solution;
+        }
+    }
+
+    size_t concurrency() const { return threads; }
+};
+}
+
+static std::string mkoptname(std::string name, int reset, int threads)
+{
+    name = "optlib_" + name;
+    if(reset) name += "_r";
+    if(threads > 1) name += "_" + std::to_string(threads);
+    return name;
+}
+
+#define IKCPPOPTX(n, t, reset, threads) static bio_ik::IKFactory::Class<bio_ik::IKOptLib<cppoptlib::t<bio_ik::IKOptLibProblem>, reset, threads>> ik_optlib_##t##reset##threads(mkoptname(n, reset, threads));
+
+//#define IKCPPOPT(n, t) static bio_ik::IKFactory::Class<bio_ik::IKOptLib<cppoptlib::t<bio_ik::IKOptLibProblem>>> ik_optlib_##t(n)
+
+#define IKCPPOPT(n, t)                                                                                                                                                                                                                                                                                                                                                                                                                                                                                             \
+    IKCPPOPTX(n, t, 0, 1)                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          \
+    IKCPPOPTX(n, t, 0, 2)                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          \
+    IKCPPOPTX(n, t, 0, 4)                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          \
+    IKCPPOPTX(n, t, 1, 1)                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          \
+    IKCPPOPTX(n, t, 1, 2)                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          \
+    IKCPPOPTX(n, t, 1, 4)
+
+#include "cppoptlib/solver/bfgssolver.h"
+IKCPPOPT("bfgs", BfgsSolver);
+
+//#include "cppoptlib/solver/cmaesbsolver.h"
+// IKCPPOPT("cmaesb", CMAesBSolver);
+
+//#include "cppoptlib/solver/cmaessolver.h"
+// IKCPPOPT("cmaes", CMAesSolver);
+
+#include "cppoptlib/solver/conjugatedgradientdescentsolver.h"
+IKCPPOPT("cgd", ConjugatedGradientDescentSolver);
+
+#include "cppoptlib/solver/gradientdescentsolver.h"
+IKCPPOPT("gd", GradientDescentSolver);
+
+#include "cppoptlib/solver/lbfgsbsolver.h"
+IKCPPOPT("lbfgsb", LbfgsbSolver);
+
+#include "cppoptlib/solver/lbfgssolver.h"
+IKCPPOPT("lbfgs", LbfgsSolver);
+
+#include "cppoptlib/solver/neldermeadsolver.h"
+IKCPPOPT("nm", NelderMeadSolver);
+
+#include "cppoptlib/solver/newtondescentsolver.h"
+IKCPPOPT("nd", NewtonDescentSolver);

src/bio_ik-kinetic-support/src/ik_evolution_1.cpp

@@ -0,0 +1,561 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016-2017, Philipp Sebastian Ruppel
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include "ik_base.h"
+
+namespace bio_ik
+{
+
+struct IKEvolution1 : IKBase
+{
+    struct Individual
+    {
+        std::vector<double> genes;
+        std::vector<double> gradients;
+        double extinction;
+        double fitness;
+    };
+
+    class HeuristicErrorTree
+    {
+        size_t variable_count, tip_count;
+        std::vector<double> table;
+        std::vector<double> chain_lengths;
+        std::vector<std::vector<double>> chain_lengths_2;
+
+    public:
+        HeuristicErrorTree() {}
+        HeuristicErrorTree(moveit::core::RobotModelConstPtr robot_model, const std::vector<std::string>& tip_names)
+        {
+            tip_count = tip_names.size();
+            variable_count = robot_model->getVariableCount();
+            table.resize(tip_count * variable_count);
+            for(size_t tip_index = 0; tip_index < tip_count; tip_index++)
+            {
+                auto& tip_name = tip_names[tip_index];
+                for(auto* link_model = robot_model->getLinkModel(tip_name); link_model; link_model = link_model->getParentLinkModel())
+                {
+                    auto* joint_model = link_model->getParentJointModel();
+                    size_t v1 = joint_model->getFirstVariableIndex();
+                    size_t vn = joint_model->getVariableCount();
+                    for(size_t variable_index = v1; variable_index < v1 + vn; variable_index++)
+                        table[variable_index * tip_count + tip_index] = 1;
+                }
+            }
+            for(size_t variable_index = 0; variable_index < variable_count; variable_index++)
+            {
+                double sum = 0;
+                for(size_t tip_index = 0; tip_index < tip_count; tip_index++)
+                    sum += table[variable_index * tip_count + tip_index];
+                if(sum > 0)
+                    for(size_t tip_index = 0; tip_index < tip_count; tip_index++)
+                        table[variable_index * tip_count + tip_index] /= sum;
+            }
+
+            chain_lengths.resize(tip_count);
+            for(size_t tip_index = 0; tip_index < tip_count; tip_index++)
+            {
+                auto& tip_name = tip_names[tip_index];
+                double chain_length = 0;
+                for(auto* link_model = robot_model->getLinkModel(tip_name); link_model; link_model = link_model->getParentLinkModel())
+                {
+                    chain_length += Frame(link_model->getJointOriginTransform()).pos.length();
+                }
+                chain_lengths[tip_index] = chain_length;
+            }
+
+            chain_lengths_2.resize(tip_count);
+            for(size_t tip_index = 0; tip_index < tip_count; tip_index++)
+            {
+                auto& tip_name = tip_names[tip_index];
+                double chain_length = 0;
+                chain_lengths_2[tip_index].resize(variable_count, 0.0);
+                for(auto* link_model = robot_model->getLinkModel(tip_name); link_model; link_model = link_model->getParentLinkModel())
+                {
+
+                    auto* joint_model = link_model->getParentJointModel();
+                    int vmin = joint_model->getFirstVariableIndex();
+                    int vmax = vmin + joint_model->getVariableCount();
+                    for(int vi = vmin; vi < vmax; vi++)
+                        chain_lengths_2[tip_index][vi] = chain_length;
+                    chain_length += Frame(link_model->getJointOriginTransform()).pos.length();
+                }
+            }
+        }
+        inline double getInfluence(size_t variable_index, size_t tip_index) const { return table[variable_index * tip_count + tip_index]; }
+        inline double getChainLength(size_t tip_index) const { return chain_lengths[tip_index]; }
+        inline double getJointVariableChainLength(size_t tip_index, size_t variable_index) const { return chain_lengths_2[tip_index][variable_index]; }
+    };
+
+    HeuristicErrorTree heuristicErrorTree;
+    std::vector<double> solution;
+    std::vector<Individual> population;
+    int populationSize, eliteCount;
+    std::vector<Individual*> tempPool;
+    std::vector<Individual> tempOffspring;
+    std::vector<double> initialGuess;
+
+    bool opt_no_wipeout;
+
+    bool linear_fitness;
+
+    void setParams(const IKParams& p)
+    {
+        opt_no_wipeout = p.opt_no_wipeout;
+        populationSize = p.population_size;
+        eliteCount = p.elite_count;
+        linear_fitness = p.linear_fitness;
+    }
+
+    bool in_final_adjustment_loop;
+
+    template <class t> inline t select(const std::vector<t>& v)
+    {
+        // FNPROFILER();
+        linear_int_distribution<size_t> d(v.size());
+        size_t index = d(rng);
+        return v[index];
+    }
+
+    inline double clip(double v, size_t i) { return modelInfo.clip(v, i); }
+
+    inline double getMutationStrength(size_t i, const Individual& parentA, const Individual& parentB)
+    {
+        double extinction = 0.5 * (parentA.extinction + parentB.extinction);
+        double span = modelInfo.getSpan(i);
+        return span * extinction;
+    }
+
+    double computeAngularScale(size_t tip_index, const Frame& tip_frame)
+    {
+        double angular_scale = sqrt(heuristicErrorTree.getChainLength(tip_index) * tip_frame.pos.length()) / M_PI;
+        return angular_scale;
+        // return 1;
+        /*double angular_scale = sqrt(heuristicErrorTree.getChainLength(tip_index) * tip_frame.pos.length()) / M_PI;
+        //double angular_scale = heuristicErrorTree.getChainLength(tip_index) * (1.0 / M_PI);
+        if(opt_angular_scale_full_circle) angular_scale *= 0.5;
+        return angular_scale;*/
+    }
+
+    double getHeuristicError(size_t variable_index, bool balanced)
+    {
+        // return 1;
+
+        double heuristic_error = 0;
+        // for(int tip_index = 0; tip_index < tipObjectives.size(); tip_index++)
+        for(int tip_index = 0; tip_index < problem.goals.size(); tip_index++)
+        {
+            double influence = heuristicErrorTree.getInfluence(variable_index, tip_index);
+            if(influence == 0) continue;
+
+            // const auto& ta = tipObjectives[tip_index];
+            const auto& ta = problem.goals[tip_index].frame;
+            const auto& tb = model.getTipFrame(tip_index);
+
+            double length = heuristicErrorTree.getJointVariableChainLength(tip_index, variable_index);
+
+            // LOG_ALWAYS("a", heuristicErrorTree.getJointVariableChainLength(tip_index, variable_index));
+
+            // double length = model.getJointVariableFrame(variable_index).pos.distance(model.getTipFrame(tip_index).pos); if(length <= 0.000000001) length = 0;
+
+            // LOG_ALWAYS("b", length);
+
+            if(modelInfo.isPrismatic(variable_index))
+            {
+                // heuristic_error += ta.pos.distance(tb.pos) * influence;
+                // if(length) heuristic_error += ta.rot.angle(tb.rot) * length * influence;
+
+                if(length)
+                {
+                    heuristic_error += ta.pos.distance(tb.pos) * influence * 0.5;
+                    heuristic_error += ta.rot.angle(tb.rot) * length * influence * 0.5;
+                }
+                else
+                {
+                    heuristic_error += ta.pos.distance(tb.pos) * influence;
+                }
+            }
+
+            if(modelInfo.isRevolute(variable_index))
+            {
+                // if(length) heuristic_error += ta.pos.distance(tb.pos) / length * influence;
+                // heuristic_error += ta.rot.angle(tb.rot) * influence;
+
+                if(length)
+                {
+                    heuristic_error += ta.pos.distance(tb.pos) / length * influence * 0.5;
+                    heuristic_error += ta.rot.angle(tb.rot) * influence * 0.5;
+                }
+                else
+                {
+                    heuristic_error += ta.rot.angle(tb.rot) * influence;
+                }
+
+                // double d = 0.0;
+                // if(length) d = std::max(d, ta.pos.distance(tb.pos) / length);
+                // d = std::max(d, ta.rot.angle(tb.rot));
+                // heuristic_error += d * influence;
+            }
+        }
+        // heuristic_error *= 0.5;
+        // LOG_ALWAYS(heuristic_error);
+        return heuristic_error;
+    }
+
+    bool in_adjustment_2, in_get_solution_fitness;
+
+    void reroll(Individual& offspring)
+    {
+        FNPROFILER();
+        // for(size_t i = 0; i < offspring.genes.size(); i++)
+        for(auto i : problem.active_variables)
+        {
+            offspring.genes[i] = random(modelInfo.getMin(i), modelInfo.getMax(i));
+
+            offspring.genes[i] = mix(offspring.genes[i], (modelInfo.getMin(i) + modelInfo.getMax(i)) * 0.5, random(0.0, 0.1));
+
+            offspring.gradients[i] = 0;
+        }
+        offspring.fitness = computeFitness(offspring.genes, false);
+    }
+
+    double computeFitness(const std::vector<double>& genes, bool balanced)
+    {
+        if(linear_fitness)
+        {
+            model.applyConfiguration(genes);
+            double fitness_sum = 0.0;
+            for(size_t goal_index = 0; goal_index < problem.goals.size(); goal_index++)
+            {
+                const auto& ta = problem.goals[goal_index].frame;
+                const auto& tb = model.getTipFrame(problem.goals[goal_index].tip_index);
+
+                double tdist = ta.pos.distance(tb.pos) / computeAngularScale(problem.goals[goal_index].tip_index, ta);
+                double rdist = ta.rot.angle(tb.rot);
+
+                fitness_sum += mix(tdist, rdist, (balanced || in_final_adjustment_loop) ? 0.5 : random());
+            }
+            return fitness_sum;
+        }
+        else
+        {
+            return IKBase::computeFitness(genes);
+        }
+    }
+
+    bool checkWipeout()
+    {
+        FNPROFILER();
+        auto& genes = population[0].genes;
+        // for(size_t i = 0; i < genes.size(); i++)
+        for(auto i : problem.active_variables)
+        {
+            double v0 = genes[i];
+            double fitness = computeFitness(genes, true);
+            double heuristicError = getHeuristicError(i, true);
+            // double heuristicError = 0.001;
+            genes[i] = modelInfo.clip(v0 + random(0, heuristicError), i);
+            double incFitness = computeFitness(genes, true);
+            genes[i] = modelInfo.clip(v0 - random(0, heuristicError), i);
+            double decFitness = computeFitness(genes, true);
+            genes[i] = v0;
+            if(incFitness < fitness || decFitness < fitness)
+            {
+                // LOG("no wipeout");
+                return false;
+            }
+        }
+        // LOG("wipeout 1");
+        return true;
+    }
+
+    void computeExtinctions()
+    {
+        double min = population.front().fitness;
+        double max = population.back().fitness;
+        for(size_t i = 0; i < populationSize; i++)
+        {
+            double grading = (double)i / (double)(populationSize - 1);
+            population[i].extinction = (population[i].fitness + min * (grading - 1)) / max;
+        }
+    }
+
+    bool tryUpdateSolution()
+    {
+        FNPROFILER();
+        double solutionFitness = computeFitness(solution, true);
+        double candidateFitness = computeFitness(population[0].genes, true);
+        // LOG_VAR(solutionFitness);
+        // LOG_VAR(candidateFitness);
+        if(candidateFitness < solutionFitness)
+        {
+            solution = population[0].genes;
+            // solution = initialGuess;
+            // for(auto i : problem.active_variables)
+            //    solution[i] = population[0].genes[i];
+            return true;
+        }
+        return false;
+    }
+
+    double getMutationProbability(const Individual& parentA, const Individual& parentB)
+    {
+        double extinction = 0.5 * (parentA.extinction + parentB.extinction);
+        double inverse = 1.0 / parentA.genes.size();
+        return extinction * (1.0 - inverse) + inverse;
+    }
+
+    void sortByFitness()
+    {
+        FNPROFILER();
+        sort(population.begin(), population.end(), [](const Individual& a, const Individual& b) { return a.fitness < b.fitness; });
+    }
+
+    double bounce(double v, int i)
+    {
+        double c = clip(v, i);
+        v = c - (v - c) * 2;
+        // v = c + c - v;
+        v = clip(v, i);
+        return v;
+    }
+
+    void reproduce(Individual& offspring, const Individual& parentA, const Individual& parentB, const Individual& prototype)
+    {
+        FNPROFILER();
+        for(size_t i = 0; i < offspring.genes.size(); i++)
+        // for(auto i : problem.active_variables)
+        {
+            offspring.genes[i] = mix(parentA.genes[i], parentB.genes[i], random());
+            offspring.genes[i] += parentA.gradients[i] * random();
+            offspring.genes[i] += parentB.gradients[i] * random();
+
+            double storage = offspring.genes[i];
+
+            if(random() < getMutationProbability(parentA, parentB)) offspring.genes[i] += random(-1, 1) * getMutationStrength(i, parentA, parentB);
+            // offspring.genes[i] += normal_random() * getMutationStrength(i, parentA, parentB);
+
+            offspring.genes[i] += mix(random() * (0.5 * (parentA.genes[i] + parentB.genes[i]) - offspring.genes[i]), random() * (prototype.genes[i] - offspring.genes[i]), random());
+
+            // offspring.genes[i] = clip(offspring.genes[i], i);
+
+            // offspring.genes[i] += fabs(offspring.genes[i] - storage) * offspring.genes[i] - (modelInfo.getMin(i) + modelInfo.getMax(i)) * 0.5;
+
+            // offspring.genes[i] = mix(offspring.genes[i], (modelInfo.getMin(i) + modelInfo.getMax(i)) * 0.5, random() * 0.1 * fabs(offspring.genes[i] - storage) / modelInfo.getSpan(i));
+
+            offspring.genes[i] = clip(offspring.genes[i], i);
+
+            // offspring.genes[i] = bounce(offspring.genes[i], i);
+
+            offspring.gradients[i] = random() * offspring.gradients[i] + offspring.genes[i] - storage;
+        }
+
+        offspring.fitness = computeFitness(offspring.genes, false);
+    }
+
+    void exploit(Individual& individual)
+    {
+        FNPROFILER();
+
+        double fitness_sum = 0;
+
+        // model.incrementalBegin(individual.genes);
+
+        for(auto i : problem.active_variables)
+        {
+            double fitness = computeFitness(individual.genes, true);
+
+            double heuristicError = getHeuristicError(i, true);
+            double v_0 = individual.genes[i];
+
+            double v_inc = clip(v_0 + random(0, heuristicError), i);
+            double v_dec = clip(v_0 - random(0, heuristicError), i);
+
+            individual.genes[i] = v_inc;
+            double inc_fitness = computeFitness(individual.genes, true);
+            individual.genes[i] = v_dec;
+            double dec_fitness = computeFitness(individual.genes, true);
+
+            if(inc_fitness < fitness && inc_fitness <= dec_fitness)
+            {
+                individual.genes[i] = v_inc;
+                individual.gradients[i] = v_0 * random() + v_inc - v_0;
+                fitness_sum += inc_fitness;
+            }
+            else if(dec_fitness < fitness && dec_fitness <= inc_fitness)
+            {
+                individual.genes[i] = v_dec;
+                individual.gradients[i] = v_0 * random() + v_dec - v_0;
+                fitness_sum += dec_fitness;
+            }
+            else
+            {
+                individual.genes[i] = v_0;
+                fitness_sum += fitness;
+            }
+        }
+
+        // model.incrementalEnd();
+
+        individual.fitness = fitness_sum / individual.genes.size();
+    }
+
+    IKEvolution1(const IKParams& p)
+        : IKBase(p)
+        , populationSize(12)
+        , eliteCount(4)
+        , in_final_adjustment_loop(false)
+        , in_adjustment_2(false)
+        , in_get_solution_fitness(false)
+    {
+        setParams(p);
+    }
+
+    void init()
+    {
+        initialGuess = problem.initial_guess;
+        solution = initialGuess;
+
+        population.resize(populationSize);
+
+        {
+            auto& p = population[0];
+            p.genes = solution;
+            p.gradients.clear();
+            p.gradients.resize(p.genes.size(), 0);
+            p.fitness = computeFitness(p.genes, false);
+        }
+
+        for(int i = 1; i < populationSize; i++)
+        {
+            auto& p = population[i];
+            p.genes = solution;
+            p.gradients.clear();
+            p.gradients.resize(p.genes.size(), 0);
+            reroll(p);
+        }
+
+        sortByFitness();
+        computeExtinctions();
+    }
+
+    void initialize(const Problem& problem)
+    {
+        IKBase::initialize(problem);
+
+        std::vector<std::string> tips;
+        for(auto tip_link_index : problem.tip_link_indices)
+            tips.push_back(params.robot_model->getLinkModelNames()[tip_link_index]);
+        heuristicErrorTree = HeuristicErrorTree(params.robot_model, tips);
+
+        init();
+    }
+
+    const std::vector<double>& getSolution() const { return solution; }
+
+    double getSolutionFitness()
+    {
+        in_get_solution_fitness = true;
+        double f = computeFitness(solution, true);
+        in_get_solution_fitness = false;
+        return f;
+    }
+
+    const std::vector<Frame>& getSolutionTipFrames()
+    {
+        model.applyConfiguration(solution);
+        return model.getTipFrames();
+    }
+
+    bool evolve()
+    {
+        FNPROFILER();
+
+        auto& offspring = tempOffspring;
+        offspring = population;
+
+        for(size_t i = 0; i < eliteCount; i++)
+        {
+            offspring[i] = population[i];
+            exploit(offspring[i]);
+        }
+
+        auto& pool = tempPool;
+        pool.resize(populationSize);
+        iota(pool.begin(), pool.end(), &population[0]);
+
+        for(size_t i = eliteCount; i < populationSize; i++)
+        {
+            if(pool.size() > 0)
+            {
+                auto& parentA = *select(pool);
+                auto& parentB = *select(pool);
+                auto& prototype = *select(pool);
+                reproduce(offspring[i], parentA, parentB, prototype);
+                if(offspring[i].fitness < parentA.fitness) pool.erase(remove(pool.begin(), pool.end(), &parentA), pool.end());
+                if(offspring[i].fitness < parentB.fitness) pool.erase(remove(pool.begin(), pool.end(), &parentB), pool.end());
+            }
+            else
+            {
+                reroll(offspring[i]);
+            }
+        }
+
+        population = offspring;
+
+        sortByFitness();
+
+        computeExtinctions();
+
+        if(tryUpdateSolution()) return true;
+        if(opt_no_wipeout) return false;
+        if(!checkWipeout()) return false;
+
+        init();
+
+        return tryUpdateSolution();
+    }
+
+    void step()
+    {
+        in_adjustment_2 = false;
+        evolve();
+    }
+
+    virtual size_t concurrency() const { return 4; }
+};
+
+static IKFactory::Class<IKEvolution1> cIKEvolution1("bio1");
+
+}

src/bio_ik-kinetic-support/src/ik_evolution_2.cpp

@@ -0,0 +1,659 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016-2017, Philipp Sebastian Ruppel
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include "ik_base.h"
+
+#ifdef ENABLE_CPP_OPTLIB
+#include "cppoptlib/solver/lbfgssolver.h"
+#endif
+
+namespace bio_ik
+{
+
+// fast evolutionary inverse kinematics
+template <int memetic> struct IKEvolution2 : IKBase
+{
+    struct Individual
+    {
+        aligned_vector<double> genes;
+        aligned_vector<double> gradients;
+        double fitness;
+    };
+
+    struct Species
+    {
+        std::vector<Individual> individuals;
+        double fitness;
+        bool improved;
+    };
+
+    std::vector<double> initial_guess;
+    std::vector<double> solution;
+    std::vector<double> temp_joint_variables;
+    double solution_fitness;
+    std::vector<Species> species;
+    std::vector<Individual> children;
+    std::vector<aligned_vector<Frame>> phenotypes, phenotypes2, phenotypes3;
+    std::vector<size_t> child_indices;
+    std::vector<double*> genotypes;
+    std::vector<Frame> phenotype;
+    std::vector<size_t> quaternion_genes;
+    aligned_vector<double> genes_min, genes_max, genes_span;
+    aligned_vector<double> gradient, temp;
+
+    IKEvolution2(const IKParams& p)
+        : IKBase(p)
+    {
+    }
+
+#ifdef ENABLE_CPP_OPTLIB
+    struct OptlibProblem : cppoptlib::Problem<double>
+    {
+        IKEvolution2* ik;
+        OptlibProblem(IKEvolution2* ik)
+            : ik(ik)
+        {
+        }
+        double value(const TVector& x)
+        {
+            const double* genes = x.data();
+            ik->model.computeApproximateMutations(1, &genes, ik->phenotypes);
+            return ik->computeCombinedFitnessActiveVariables(ik->phenotypes[0], genes);
+        }
+    };
+    typedef cppoptlib::LbfgsSolver<OptlibProblem> OptlibSolver;
+    std::shared_ptr<OptlibSolver> optlib_solver;
+    std::shared_ptr<OptlibProblem> optlib_problem;
+    typename OptlibSolver::TVector optlib_vector;
+#endif
+
+    void genesToJointVariables(const Individual& individual, std::vector<double>& variables)
+    {
+        auto& genes = individual.genes;
+        variables.resize(params.robot_model->getVariableCount());
+        for(size_t i = 0; i < problem.active_variables.size(); i++)
+            variables[problem.active_variables[i]] = genes[i];
+    }
+
+    const std::vector<double>& getSolution() const { return solution; }
+
+    void initialize(const Problem& problem)
+    {
+        BLOCKPROFILER("initialization");
+
+        IKBase::initialize(problem);
+
+        // init list of quaternion joint genes to be normalized during each mutation
+        quaternion_genes.clear();
+        for(size_t igene = 0; igene < problem.active_variables.size(); igene++)
+        {
+            size_t ivar = problem.active_variables[igene];
+            auto* joint_model = params.robot_model->getJointOfVariable(ivar);
+            if(joint_model->getFirstVariableIndex() + 3 != ivar) continue;
+            if(joint_model->getType() != moveit::core::JointModel::FLOATING) continue;
+            quaternion_genes.push_back(igene);
+        }
+
+        // set solution to initial guess
+        initial_guess = problem.initial_guess;
+        solution = initial_guess;
+        solution_fitness = computeFitness(solution);
+
+        // init temporary buffer with positions of inactive joints
+        temp_joint_variables = initial_guess;
+
+        // params
+        size_t population_size = 2;
+        size_t child_count = 16;
+
+        // initialize population on current island
+        species.resize(2);
+        for(auto& s : species)
+        {
+            // create individuals
+            s.individuals.resize(population_size);
+
+            // initialize first individual
+            {
+                auto& v = s.individuals[0];
+
+                // init genes
+                v.genes.resize(problem.active_variables.size());
+                // if(thread_index == 0) // on first island?
+                // if(thread_index % 2 == 0) // on every second island...
+                if(1)
+                {
+                    // set to initial_guess
+                    for(size_t i = 0; i < v.genes.size(); i++)
+                        v.genes[i] = initial_guess[problem.active_variables[i]];
+                }
+                else
+                {
+                    // initialize populations on other islands randomly
+                    for(size_t i = 0; i < v.genes.size(); i++)
+                        v.genes[i] = random(modelInfo.getMin(problem.active_variables[i]), modelInfo.getMax(problem.active_variables[i]));
+                }
+
+                // set gradients to zero
+                v.gradients.clear();
+                v.gradients.resize(problem.active_variables.size(), 0);
+            }
+
+            // clone first individual
+            for(size_t i = 1; i < s.individuals.size(); i++)
+            {
+                s.individuals[i].genes = s.individuals[0].genes;
+                s.individuals[i].gradients = s.individuals[0].gradients;
+            }
+        }
+
+        // space for child population
+        children.resize(population_size + child_count);
+        for(auto& child : children)
+        {
+            child.genes.resize(problem.active_variables.size());
+            child.gradients.resize(problem.active_variables.size());
+        }
+
+        // init gene infos
+        // if(genes_min.empty())
+        {
+            genes_min.resize(problem.active_variables.size());
+            genes_max.resize(problem.active_variables.size());
+            genes_span.resize(problem.active_variables.size());
+            for(size_t i = 0; i < problem.active_variables.size(); i++)
+            {
+                genes_min[i] = modelInfo.getClipMin(problem.active_variables[i]);
+                genes_max[i] = modelInfo.getClipMax(problem.active_variables[i]);
+                genes_span[i] = modelInfo.getSpan(problem.active_variables[i]);
+            }
+        }
+
+        /*
+        // init chain mutation masks
+        chain_mutation_masks.resize(chain_mutation_mask_count);
+        for(auto& chain_mutation_mask : chain_mutation_masks)
+        {
+            temp_mutation_chain.clear();
+            if(problem.tip_link_indices.size() > 1)
+            {
+                for(auto* chain_link = params.robot_model->getLinkModel(random_element(problem.tip_link_indices)); chain_link; chain_link = chain_link->getParentLinkModel())
+                    temp_mutation_chain.push_back(chain_link);
+                temp_mutation_chain.resize(random_index(temp_mutation_chain.size()) + 1);
+            }
+
+            temp_chain_mutation_var_mask.resize(params.robot_model->getVariableCount());
+            for(auto& m : temp_chain_mutation_var_mask) m = 0;
+            for(auto* chain_link : temp_mutation_chain)
+            {
+                auto* chain_joint = chain_link->getParentJointModel();
+                for(size_t ivar = chain_joint->getFirstVariableIndex(); ivar < chain_joint->getFirstVariableIndex() + chain_joint->getVariableCount(); ivar++)
+                    temp_chain_mutation_var_mask[ivar] = 1;
+            }
+
+            chain_mutation_mask.resize(problem.active_variables.size());
+            for(size_t igene = 0; igene < problem.active_variables.size(); igene++)
+                chain_mutation_mask[igene] = temp_chain_mutation_var_mask[problem.active_variables[igene]];
+        }
+        */
+    }
+
+    /*
+    const size_t chain_mutation_mask_count = 256;
+    std::vector<std::vector<int>> chain_mutation_masks;
+    std::vector<const moveit::core::LinkModel*> temp_mutation_chain;
+    std::vector<int> temp_chain_mutation_var_mask;
+    */
+
+    // aligned_vector<double> rmask;
+
+    // create offspring and mutate
+    __attribute__((hot)) __attribute__((noinline))
+    //__attribute__((target_clones("avx2", "avx", "sse2", "default")))
+    //__attribute__((target("avx")))
+    void
+    reproduce(const std::vector<Individual>& population)
+    {
+        const auto __attribute__((aligned(32)))* __restrict__ genes_span = this->genes_span.data();
+        const auto __attribute__((aligned(32)))* __restrict__ genes_min = this->genes_min.data();
+        const auto __attribute__((aligned(32)))* __restrict__ genes_max = this->genes_max.data();
+
+        auto gene_count = children[0].genes.size();
+
+        size_t s = (children.size() - population.size()) * gene_count + children.size() * 4 + 4;
+
+        auto* __restrict__ rr = fast_random_gauss_n(s);
+        rr = (const double*)(((size_t)rr + 3) / 4 * 4);
+
+        /*rmask.resize(s);
+        for(auto& m : rmask) m = fast_random() < 0.1 ? 1.0 : 0.0;
+        double* dm = rmask.data();*/
+
+        for(size_t child_index = population.size(); child_index < children.size(); child_index++)
+        {
+            double mutation_rate = (1 << fast_random_index(16)) * (1.0 / (1 << 23));
+            auto& parent = population[0];
+            auto& parent2 = population[1];
+            double fmix = (child_index % 2 == 0) * 0.2;
+            double gradient_factor = child_index % 3;
+
+            auto __attribute__((aligned(32)))* __restrict__ parent_genes = parent.genes.data();
+            auto __attribute__((aligned(32)))* __restrict__ parent_gradients = parent.gradients.data();
+
+            auto __attribute__((aligned(32)))* __restrict__ parent2_genes = parent2.genes.data();
+            auto __attribute__((aligned(32)))* __restrict__ parent2_gradients = parent2.gradients.data();
+
+            auto& child = children[child_index];
+
+            auto __attribute__((aligned(32)))* __restrict__ child_genes = child.genes.data();
+            auto __attribute__((aligned(32)))* __restrict__ child_gradients = child.gradients.data();
+
+#pragma unroll
+#pragma omp simd aligned(genes_span : 32), aligned(genes_min : 32), aligned(genes_max : 32), aligned(parent_genes : 32), aligned(parent_gradients : 32), aligned(parent2_genes : 32), aligned(parent2_gradients : 32), aligned(child_genes : 32), aligned(child_gradients : 32) aligned(rr : 32)
+            for(size_t gene_index = 0; gene_index < gene_count; gene_index++)
+            {
+                // double mutation_rate = (1 << fast_random_index(16)) * (1.0 / (1 << 23));
+
+                double r = rr[gene_index];
+                // r *= dm[gene_index];
+                double f = mutation_rate * genes_span[gene_index];
+                double gene = parent_genes[gene_index];
+                double parent_gene = gene;
+                gene += r * f;
+                double parent_gradient = mix(parent_gradients[gene_index], parent2_gradients[gene_index], fmix);
+                double gradient = parent_gradient * gradient_factor;
+                gene += gradient;
+                gene = clamp(gene, genes_min[gene_index], genes_max[gene_index]);
+                child_genes[gene_index] = gene;
+                child_gradients[gene_index] = mix(parent_gradient, gene - parent_gene, 0.3);
+            }
+            rr += (gene_count + 3) / 4 * 4;
+            // dm += (gene_count + 3) / 4 * 4;
+
+            /*if(problem.tip_link_indices.size() > 1)
+            {
+                if(fast_random() < 0.5)
+                {
+                    auto& mask = chain_mutation_masks[fast_random_index(chain_mutation_mask_count)];
+                    for(size_t gene_index = 0; gene_index < gene_count; gene_index++)
+                    {
+                        if(!mask[gene_index])
+                        {
+                            child_genes[gene_index] = parent_genes[gene_index];
+                            child_gradients[gene_index] = parent_gradients[gene_index];
+                        }
+                    }
+                }
+            }*/
+
+            for(auto quaternion_gene_index : quaternion_genes)
+            {
+                auto& qpos = (*(Quaternion*)&(children[child_index].genes[quaternion_gene_index]));
+                normalizeFast(qpos);
+            }
+        }
+    }
+
+    void step()
+    {
+        FNPROFILER();
+
+        for(size_t ispecies = 0; ispecies < species.size(); ispecies++)
+        {
+            auto& species = this->species[ispecies];
+            auto& population = species.individuals;
+
+            {
+                BLOCKPROFILER("evolution");
+
+                // initialize forward kinematics approximator
+                genesToJointVariables(species.individuals[0], temp_joint_variables);
+                {
+                    BLOCKPROFILER("fk");
+                    model.applyConfiguration(temp_joint_variables);
+                    model.initializeMutationApproximator(problem.active_variables);
+                }
+
+                // run evolution for a few generations
+                size_t generation_count = 16;
+                if(memetic) generation_count = 8;
+                for(size_t generation = 0; generation < generation_count; generation++)
+                {
+                    // BLOCKPROFILER("evolution");
+
+                    if(canceled) break;
+
+                    // reproduction
+                    {
+                        BLOCKPROFILER("reproduction");
+                        reproduce(population);
+                    }
+
+                    size_t child_count = children.size();
+
+                    // pre-selection by secondary objectives
+                    if(problem.secondary_goals.size())
+                    {
+                        BLOCKPROFILER("pre-selection");
+                        child_count = random_index(children.size() - population.size() - 1) + 1 + population.size();
+                        for(size_t child_index = population.size(); child_index < children.size(); child_index++)
+                        {
+                            children[child_index].fitness = computeSecondaryFitnessActiveVariables(children[child_index].genes.data());
+                        }
+                        {
+                            BLOCKPROFILER("pre-selection sort");
+                            std::sort(children.begin() + population.size(), children.end(), [](const Individual& a, const Individual& b) { return a.fitness < b.fitness; });
+                        }
+                    }
+
+                    // keep parents
+                    {
+                        BLOCKPROFILER("keep alive");
+                        for(size_t i = 0; i < population.size(); i++)
+                        {
+                            children[i].genes = population[i].genes;
+                            children[i].gradients = population[i].gradients;
+                        }
+                    }
+
+                    // genotype-phenotype mapping
+                    {
+                        BLOCKPROFILER("phenotype");
+                        size_t gene_count = children[0].genes.size();
+                        genotypes.resize(child_count);
+                        for(size_t i = 0; i < child_count; i++)
+                            genotypes[i] = children[i].genes.data();
+                        model.computeApproximateMutations(child_count, genotypes.data(), phenotypes);
+                    }
+
+                    // fitness
+                    {
+                        BLOCKPROFILER("fitness");
+                        for(size_t child_index = 0; child_index < child_count; child_index++)
+                        {
+                            children[child_index].fitness = computeFitnessActiveVariables(phenotypes[child_index], genotypes[child_index]);
+                        }
+                    }
+
+                    // selection
+                    {
+                        BLOCKPROFILER("selection");
+                        child_indices.resize(child_count);
+                        for(size_t i = 0; i < child_count; i++)
+                            child_indices[i] = i;
+                        for(size_t i = 0; i < population.size(); i++)
+                        {
+                            size_t jmin = i;
+                            double fmin = children[child_indices[i]].fitness;
+                            for(size_t j = i + 1; j < child_count; j++)
+                            {
+                                double f = children[child_indices[j]].fitness;
+                                if(f < fmin) jmin = j, fmin = f;
+                            }
+                            std::swap(child_indices[i], child_indices[jmin]);
+                        }
+                        for(size_t i = 0; i < population.size(); i++)
+                        {
+                            std::swap(population[i].genes, children[child_indices[i]].genes);
+                            std::swap(population[i].gradients, children[child_indices[i]].gradients);
+                        }
+                    }
+                }
+            }
+
+            // memetic optimization
+            {
+                BLOCKPROFILER("memetics");
+
+                if(memetic == 'q' || memetic == 'l')
+                {
+
+                    // init
+                    auto& individual = population[0];
+                    gradient.resize(problem.active_variables.size());
+                    if(genotypes.empty()) genotypes.emplace_back();
+                    phenotypes2.resize(1);
+                    phenotypes3.resize(1);
+
+                    // differentiation step size
+                    double dp = 0.0000001;
+                    if(fast_random() < 0.5) dp = -dp;
+
+                    for(size_t generation = 0; generation < 8; generation++)
+                    // for(size_t generation = 0; generation < 32; generation++)
+                    {
+
+                        if(canceled) break;
+
+                        // compute gradient
+                        temp = individual.genes;
+                        genotypes[0] = temp.data();
+                        model.computeApproximateMutations(1, genotypes.data(), phenotypes2);
+                        double f2p = computeFitnessActiveVariables(phenotypes2[0], genotypes[0]);
+                        double fa = f2p + computeSecondaryFitnessActiveVariables(genotypes[0]);
+                        for(size_t i = 0; i < problem.active_variables.size(); i++)
+                        {
+                            double* pp = &(genotypes[0][i]);
+                            genotypes[0][i] = individual.genes[i] + dp;
+                            model.computeApproximateMutation1(problem.active_variables[i], +dp, phenotypes2[0], phenotypes3[0]);
+                            double fb = computeCombinedFitnessActiveVariables(phenotypes3[0], genotypes[0]);
+                            genotypes[0][i] = individual.genes[i];
+                            double d = fb - fa;
+                            gradient[i] = d;
+                        }
+
+                        // normalize gradient
+                        double sum = dp * dp;
+                        for(size_t i = 0; i < problem.active_variables.size(); i++)
+                            sum += fabs(gradient[i]);
+                        double f = 1.0 / sum * dp;
+                        for(size_t i = 0; i < problem.active_variables.size(); i++)
+                            gradient[i] *= f;
+
+                        // sample support points for line search
+                        for(size_t i = 0; i < problem.active_variables.size(); i++)
+                            genotypes[0][i] = individual.genes[i] - gradient[i];
+                        model.computeApproximateMutations(1, genotypes.data(), phenotypes3);
+                        double f1 = computeCombinedFitnessActiveVariables(phenotypes3[0], genotypes[0]);
+
+                        double f2 = fa;
+
+                        for(size_t i = 0; i < problem.active_variables.size(); i++)
+                            genotypes[0][i] = individual.genes[i] + gradient[i];
+                        model.computeApproximateMutations(1, genotypes.data(), phenotypes3);
+                        double f3 = computeCombinedFitnessActiveVariables(phenotypes3[0], genotypes[0]);
+
+                        // quadratic step size
+                        if(memetic == 'q')
+                        {
+
+                            // compute step size
+                            double v1 = (f2 - f1); // f / j
+                            double v2 = (f3 - f2); // f / j
+                            double v = (v1 + v2) * 0.5; // f / j
+                            double a = (v1 - v2); // f / j^2
+                            double step_size = v / a; // (f / j) / (f / j^2) = f / j / f * j * j = j
+
+                            // double v1 = (f2 - f1) / dp;
+                            // double v2 = (f3 - f2) / dp;
+                            // double v = (v1 + v2) * 0.5;
+                            // double a = (v2 - v1) / dp;
+                            // // v * x + a * x * x = 0;
+                            // // v = - a * x
+                            // // - v / a = x
+                            // // x = -v / a;
+                            // double step_size = -v / a / dp;
+
+                            // for(double f : { 1.0, 0.5, 0.25 })
+                            {
+
+                                double f = 1.0;
+
+                                // move by step size along gradient and compute fitness
+                                for(size_t i = 0; i < problem.active_variables.size(); i++)
+                                    genotypes[0][i] = modelInfo.clip(individual.genes[i] + gradient[i] * step_size * f, problem.active_variables[i]);
+                                model.computeApproximateMutations(1, genotypes.data(), phenotypes2);
+                                double f4p = computeFitnessActiveVariables(phenotypes2[0], genotypes[0]);
+
+                                // accept new position if better
+                                if(f4p < f2p)
+                                {
+                                    individual.genes = temp;
+                                    continue;
+                                }
+                                else
+                                {
+                                    break;
+                                }
+                            }
+
+                            // break;
+                        }
+
+                        // linear step size
+                        if(memetic == 'l')
+                        {
+
+                            // compute step size
+                            double cost_diff = (f3 - f1) * 0.5; // f / j
+                            double step_size = f2 / cost_diff; // f / (f / j) = j
+
+                            // move by step size along gradient and compute fitness
+                            for(size_t i = 0; i < problem.active_variables.size(); i++)
+                                temp[i] = modelInfo.clip(individual.genes[i] - gradient[i] * step_size, problem.active_variables[i]);
+                            model.computeApproximateMutations(1, genotypes.data(), phenotypes2);
+                            double f4p = computeFitnessActiveVariables(phenotypes2[0], genotypes[0]);
+
+                            // accept new position if better
+                            if(f4p < f2p)
+                            {
+                                individual.genes = temp;
+                                continue;
+                            }
+                            else
+                            {
+                                break;
+                            }
+                        }
+                    }
+                }
+
+#ifdef ENABLE_CPP_OPTLIB
+                // cppoptlib::LbfgsSolver memetic test
+                if(memetic == 'o')
+                {
+                    auto& individual = population[0];
+
+                    // create cppoptlib solver and cppoptlib problem, if not yet existing
+                    if(!optlib_solver)
+                    {
+                        optlib_solver = std::make_shared<OptlibSolver>();
+                        cppoptlib::Criteria<double> crit;
+                        crit.iterations = 4;
+                        optlib_solver->setStopCriteria(crit);
+                        optlib_problem = std::make_shared<OptlibProblem>(this);
+                    }
+
+                    // init starting point
+                    optlib_vector.resize(problem.active_variables.size());
+                    for(size_t i = 0; i < problem.active_variables.size(); i++)
+                        optlib_vector[i] = individual.genes[i];
+
+                    // solve
+                    optlib_solver->minimize(*optlib_problem, optlib_vector);
+
+                    // get result
+                    for(size_t i = 0; i < problem.active_variables.size(); i++)
+                        individual.genes[i] = modelInfo.clip(optlib_vector[i], problem.active_variables[i]);
+                }
+#endif
+            }
+        }
+
+        {
+            BLOCKPROFILER("species");
+
+            // compute species fitness
+            for(auto& species : this->species)
+            {
+                genesToJointVariables(species.individuals[0], temp_joint_variables);
+                double fitness = computeFitness(temp_joint_variables);
+                species.improved = (fitness != species.fitness);
+                species.fitness = fitness;
+            }
+
+            // sort species by fitness
+            std::sort(species.begin(), species.end(), [](const Species& a, const Species& b) { return a.fitness < b.fitness; });
+
+            // wipeouts
+            for(size_t species_index = 1; species_index < species.size(); species_index++)
+            {
+                if(fast_random() < 0.1 || !species[species_index].improved)
+                // if(fast_random() < 0.05 || !species[species_index].improved)
+                {
+                    {
+                        auto& individual = species[species_index].individuals[0];
+
+                        for(size_t i = 0; i < individual.genes.size(); i++)
+                            individual.genes[i] = random(modelInfo.getMin(problem.active_variables[i]), modelInfo.getMax(problem.active_variables[i]));
+
+                        for(auto& v : individual.gradients)
+                            v = 0;
+                    }
+                    for(size_t i = 0; i < species[species_index].individuals.size(); i++)
+                        species[species_index].individuals[i] = species[species_index].individuals[0];
+                }
+            }
+
+            // update solution
+            if(species[0].fitness < solution_fitness)
+            {
+                genesToJointVariables(species[0].individuals[0], solution);
+                solution_fitness = species[0].fitness;
+            }
+        }
+    }
+
+    // number of islands
+    virtual size_t concurrency() const { return 4; }
+};
+
+static IKFactory::Class<IKEvolution2<0>> bio2("bio2");
+static IKFactory::Class<IKEvolution2<'q'>> bio2_memetic("bio2_memetic");
+static IKFactory::Class<IKEvolution2<'l'>> bio2_memetic_l("bio2_memetic_l");
+
+#ifdef ENABLE_CPP_OPTLIB
+static IKFactory::Class<IKEvolution2<'o'>> bio2_memetic_0("bio2_memetic_lbfgs");
+#endif
+}

src/bio_ik-kinetic-support/src/ik_gradient.cpp

@@ -0,0 +1,289 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016-2017, Philipp Sebastian Ruppel
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include "ik_base.h"
+
+namespace bio_ik
+{
+
+// pseudoinverse jacobian solver
+// (mainly for testing RobotFK_Jacobian::computeJacobian)
+template <class BASE> struct IKJacobianBase : BASE
+{
+    using BASE::modelInfo;
+    using BASE::model;
+    using BASE::params;
+    using BASE::computeFitness;
+    using BASE::problem;
+
+    std::vector<Frame> tipObjectives;
+
+    Eigen::VectorXd tip_diffs;
+    Eigen::VectorXd joint_diffs;
+    Eigen::MatrixXd jacobian;
+    std::vector<Frame> tip_frames_temp;
+
+    IKJacobianBase(const IKParams& p)
+        : BASE(p)
+    {
+    }
+
+    void initialize(const Problem& problem)
+    {
+        BASE::initialize(problem);
+        tipObjectives.resize(problem.tip_link_indices.size());
+        for(auto& goal : problem.goals)
+            tipObjectives[goal.tip_index] = goal.frame;
+    }
+
+    void optimizeJacobian(std::vector<double>& solution)
+    {
+        FNPROFILER();
+
+        int tip_count = problem.tip_link_indices.size();
+        tip_diffs.resize(tip_count * 6);
+        joint_diffs.resize(problem.active_variables.size());
+
+        // compute fk
+        model.applyConfiguration(solution);
+
+        double translational_scale = 1;
+        double rotational_scale = 1;
+
+        // compute goal diffs
+        tip_frames_temp = model.getTipFrames();
+        for(int itip = 0; itip < tip_count; itip++)
+        {
+            auto twist = frameTwist(tip_frames_temp[itip], tipObjectives[itip]);
+            tip_diffs(itip * 6 + 0) = twist.vel.x() * translational_scale;
+            tip_diffs(itip * 6 + 1) = twist.vel.y() * translational_scale;
+            tip_diffs(itip * 6 + 2) = twist.vel.z() * translational_scale;
+            tip_diffs(itip * 6 + 3) = twist.rot.x() * rotational_scale;
+            tip_diffs(itip * 6 + 4) = twist.rot.y() * rotational_scale;
+            tip_diffs(itip * 6 + 5) = twist.rot.z() * rotational_scale;
+        }
+
+        // compute jacobian
+        {
+            model.computeJacobian(problem.active_variables, jacobian);
+            int icol = 0;
+            for(auto ivar : problem.active_variables)
+            {
+                for(size_t itip = 0; itip < tip_count; itip++)
+                {
+                    jacobian(itip * 6 + 0, icol) *= translational_scale;
+                    jacobian(itip * 6 + 1, icol) *= translational_scale;
+                    jacobian(itip * 6 + 2, icol) *= translational_scale;
+                    jacobian(itip * 6 + 3, icol) *= rotational_scale;
+                    jacobian(itip * 6 + 4, icol) *= rotational_scale;
+                    jacobian(itip * 6 + 5, icol) *= rotational_scale;
+                }
+                icol++;
+            }
+        }
+
+        // get pseudoinverse and multiply
+        joint_diffs = jacobian.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(tip_diffs);
+        // joint_diffs = (jacobian.transpose() * jacobian).ldlt().solve(jacobian.transpose() * tip_diffs);
+
+        // apply joint deltas and clip
+        {
+            int icol = 0;
+            for(auto ivar : problem.active_variables)
+            {
+                auto v = solution[ivar] + joint_diffs(icol);
+                if(!std::isfinite(v)) continue;
+                v = modelInfo.clip(v, ivar);
+                solution[ivar] = v;
+                icol++;
+            }
+        }
+    }
+};
+
+// simple gradient descent
+template <int if_stuck, size_t threads> struct IKGradientDescent : IKBase
+{
+    std::vector<double> solution, best_solution, gradient, temp;
+    bool reset;
+
+    IKGradientDescent(const IKParams& p)
+        : IKBase(p)
+    {
+    }
+
+    void initialize(const Problem& problem)
+    {
+        IKBase::initialize(problem);
+        solution = problem.initial_guess;
+        if(thread_index > 0)
+            for(auto& vi : problem.active_variables)
+                solution[vi] = random(modelInfo.getMin(vi), modelInfo.getMax(vi));
+        best_solution = solution;
+        reset = false;
+    }
+
+    const std::vector<double>& getSolution() const { return best_solution; }
+
+    void step()
+    {
+        // random reset if stuck
+        if(reset)
+        {
+            reset = false;
+            for(auto& vi : problem.active_variables)
+                solution[vi] = random(modelInfo.getMin(vi), modelInfo.getMax(vi));
+        }
+
+        // compute gradient direction
+        temp = solution;
+        double jd = 0.0001;
+        gradient.resize(solution.size(), 0);
+        for(auto ivar : problem.active_variables)
+        {
+            temp[ivar] = solution[ivar] - jd;
+            double p1 = computeFitness(temp);
+
+            temp[ivar] = solution[ivar] + jd;
+            double p3 = computeFitness(temp);
+
+            temp[ivar] = solution[ivar];
+
+            gradient[ivar] = p3 - p1;
+        }
+
+        // normalize gradient direction
+        double sum = 0.0001;
+        for(auto ivar : problem.active_variables)
+            sum += fabs(gradient[ivar]);
+        double f = 1.0 / sum * jd;
+        for(auto ivar : problem.active_variables)
+            gradient[ivar] *= f;
+
+        // initialize line search
+        temp = solution;
+
+        for(auto ivar : problem.active_variables)
+            temp[ivar] = solution[ivar] - gradient[ivar];
+        double p1 = computeFitness(temp);
+
+        // for(auto ivar : problem.active_variables) temp[ivar] = solution[ivar];
+        // double p2 = computeFitness(temp);
+
+        for(auto ivar : problem.active_variables)
+            temp[ivar] = solution[ivar] + gradient[ivar];
+        double p3 = computeFitness(temp);
+
+        double p2 = (p1 + p3) * 0.5;
+
+        // linear step size estimation
+        double cost_diff = (p3 - p1) * 0.5;
+        double joint_diff = p2 / cost_diff;
+
+        // apply optimization step
+        // (move along gradient direction by estimated step size)
+        for(auto ivar : problem.active_variables)
+            temp[ivar] = modelInfo.clip(solution[ivar] - gradient[ivar] * joint_diff, ivar);
+
+        if(if_stuck == 'c')
+        {
+            // always accept solution and continue
+            solution = temp;
+        }
+        else
+        {
+            // has solution improved?
+            if(computeFitness(temp) < computeFitness(solution))
+            {
+                // solution improved -> accept solution
+                solution = temp;
+            }
+            else
+            {
+                if(if_stuck == 'r')
+                {
+                    // reset if stuck
+                    reset = true;
+                }
+            }
+        }
+
+        // update best solution
+        if(computeFitness(solution) < computeFitness(best_solution)) best_solution = solution;
+    }
+
+    size_t concurrency() const { return threads; }
+};
+
+static IKFactory::Class<IKGradientDescent<' ', 1>> gd("gd");
+static IKFactory::Class<IKGradientDescent<' ', 2>> gd_2("gd_2");
+static IKFactory::Class<IKGradientDescent<' ', 4>> gd_4("gd_4");
+static IKFactory::Class<IKGradientDescent<' ', 8>> gd_8("gd_8");
+
+static IKFactory::Class<IKGradientDescent<'r', 1>> gd_r("gd_r");
+static IKFactory::Class<IKGradientDescent<'r', 2>> gd_2_r("gd_r_2");
+static IKFactory::Class<IKGradientDescent<'r', 4>> gd_4_r("gd_r_4");
+static IKFactory::Class<IKGradientDescent<'r', 8>> gd_8_r("gd_r_8");
+
+static IKFactory::Class<IKGradientDescent<'c', 1>> gd_c("gd_c");
+static IKFactory::Class<IKGradientDescent<'c', 2>> gd_2_c("gd_c_2");
+static IKFactory::Class<IKGradientDescent<'c', 4>> gd_4_c("gd_c_4");
+static IKFactory::Class<IKGradientDescent<'c', 8>> gd_8_c("gd_c_8");
+
+// pseudoinverse jacobian only
+template <size_t threads> struct IKJacobian : IKJacobianBase<IKBase>
+{
+    using IKBase::initialize;
+    std::vector<double> solution;
+    IKJacobian(const IKParams& p)
+        : IKJacobianBase<IKBase>(p)
+    {
+    }
+    void initialize(const Problem& problem)
+    {
+        IKJacobianBase<IKBase>::initialize(problem);
+        solution = problem.initial_guess;
+        if(thread_index > 0)
+            for(auto& vi : problem.active_variables)
+                solution[vi] = random(modelInfo.getMin(vi), modelInfo.getMax(vi));
+    }
+    const std::vector<double>& getSolution() const { return solution; }
+    void step() { optimizeJacobian(solution); }
+    size_t concurrency() const { return threads; }
+};
+static IKFactory::Class<IKJacobian<1>> jac("jac");
+static IKFactory::Class<IKJacobian<2>> jac_2("jac_2");
+static IKFactory::Class<IKJacobian<4>> jac_4("jac_4");
+static IKFactory::Class<IKJacobian<8>> jac_8("jac_8");
+}

src/bio_ik-kinetic-support/src/ik_neural.cpp

@@ -0,0 +1,690 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016-2017, Philipp Sebastian Ruppel
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include "ik_base.h"
+
+#include <fann.h>
+#include <fann_cpp.h>
+
+#include <mutex>
+
+#define LOG_LIST(l)                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                \
+    {                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                              \
+        LOG(#l "[]");                                                                                                                                                                                                                                                                                                                                                                                                                                                                                              \
+        for(std::size_t i = 0; i < l.size(); i++)                                                                                                                                                                                                                                                                                                                                                                                                                                                                  \
+        {                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          \
+            LOG(#l "[" + std::to_string(i) + "]", l[i]);                                                                                                                                                                                                                                                                                                                                                                                                                                                           \
+        }                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          \
+    }
+
+namespace bio_ik
+{
+
+/*
+static inline KDL::Twist toTwist(const Frame& frame)
+{
+    KDL::Twist t;
+    t.vel.x(frame.pos.x());
+    t.vel.y(frame.pos.y());
+    t.vel.z(frame.pos.z());
+    auto r = frame.rot.getAxis() * frame.rot.getAngle();
+    t.rot.x(r.x());
+    t.rot.y(r.y());
+    t.rot.z(r.z());
+    return t;
+}
+
+static inline KDL::Twist frameTwist(const Frame& a, const Frame& b)
+{
+    auto frame = inverse(a) * b;
+    KDL::Twist t;
+    t.vel.x(frame.pos.x());
+    t.vel.y(frame.pos.y());
+    t.vel.z(frame.pos.z());
+    auto r = frame.rot.getAxis() * frame.rot.getAngle();
+    t.rot.x(r.x());
+    t.rot.y(r.y());
+    t.rot.z(r.z());
+    return t;
+}
+*/
+
+struct IKNeural : IKBase
+{
+    std::vector<double> solution;
+    FANN::neural_net net;
+
+    std::vector<std::pair<fann_type, fann_type>> input_minmax, output_minmax;
+
+    static void find_minmax(const std::vector<fann_type>& values, std::vector<std::pair<fann_type, fann_type>>& minmax)
+    {
+        for(size_t i = 0; i < minmax.size(); i++)
+        {
+            minmax[i] = std::make_pair(values[i], values[i]);
+        }
+        for(size_t i = 0; i < values.size(); i++)
+        {
+            auto& v = values[i];
+            auto& p = minmax[i % minmax.size()];
+            p.first = std::min(p.first, v);
+            p.second = std::max(p.second, v);
+        }
+    }
+
+    static void normalize(std::vector<fann_type>& values, const std::vector<std::pair<fann_type, fann_type>>& minmax)
+    {
+        for(size_t i = 0; i < values.size(); i++)
+        {
+            auto& v = values[i];
+            auto& p = minmax[i % minmax.size()];
+            v = (v - p.first) / (p.second - p.first);
+        }
+    }
+
+    static void denormalize(std::vector<fann_type>& values, const std::vector<std::pair<fann_type, fann_type>>& minmax)
+    {
+        for(size_t i = 0; i < values.size(); i++)
+        {
+            auto& v = values[i];
+            auto& p = minmax[i % minmax.size()];
+            v = v * (p.second - p.first) + p.first;
+        }
+    }
+
+    unsigned int input_count, output_count;
+
+    IKNeural(const IKParams& p)
+        : IKBase(p)
+    {
+        trained = false;
+    }
+
+    bool trained;
+
+    void train()
+    {
+        trained = true;
+
+        input_count = problem.active_variables.size() + problem.tip_link_indices.size() * 6;
+        output_count = problem.active_variables.size();
+
+        LOG_VAR(input_count);
+        LOG_VAR(output_count);
+
+        // std::vector<unsigned int> levels = {input_count, input_count, input_count, output_count};
+
+        // std::vector<unsigned int> levels = {input_count, input_count + output_count, output_count};
+
+        // std::vector<unsigned int> levels = {input_count, input_count + output_count, input_count + output_count, output_count};
+
+        // std::vector<unsigned int> levels = {input_count, 100, output_count};
+
+        std::vector<unsigned int> levels = {input_count, 50, output_count};
+
+        net.create_standard_array(levels.size(), levels.data());
+
+        size_t var_count = params.robot_model->getVariableCount();
+        std::vector<double> state1(var_count), state2(var_count);
+        std::vector<Frame> frames1, frames2;
+
+        std::vector<fann_type> inputs, outputs;
+        std::vector<fann_type*> input_pp, output_pp;
+
+        LOG("neuro ik generating training data");
+
+        unsigned int samples = 10000;
+
+        for(size_t iter = 0; iter < samples; iter++)
+        {
+            for(size_t ivar = 0; ivar < var_count; ivar++)
+            {
+                state1[ivar] = random(modelInfo.getMin(ivar), modelInfo.getMax(ivar));
+                state1[ivar] = modelInfo.clip(state1[ivar], ivar);
+                // state2[ivar] = modelInfo.clip(state1[ivar] + random_gauss() * modelInfo.getSpan(ivar), ivar);
+                state2[ivar] = modelInfo.clip(state1[ivar] + random_gauss() * 0.1, ivar);
+            }
+
+            model.applyConfiguration(state1);
+            frames1 = model.getTipFrames();
+            model.applyConfiguration(state2);
+            frames2 = model.getTipFrames();
+
+            for(auto ivar : problem.active_variables)
+            {
+                inputs.push_back(state1[ivar]);
+                outputs.push_back(state2[ivar] - state1[ivar]);
+            }
+
+            for(size_t itip = 0; itip < problem.tip_link_indices.size(); itip++)
+            {
+                double translational_scale = 1.0;
+                double rotational_scale = 1.0;
+
+                // Frame diff = inverse(frames1[itip]) * frames2[itip];
+                // auto twist = toTwist(diff);
+                auto twist = frameTwist(frames1[itip], frames2[itip]);
+
+                inputs.push_back(frames2[itip].pos.x() - frames1[itip].pos.x());
+                inputs.push_back(frames2[itip].pos.y() - frames1[itip].pos.y());
+                inputs.push_back(frames2[itip].pos.z() - frames1[itip].pos.z());
+
+                inputs.push_back(twist.rot.x() * rotational_scale);
+                inputs.push_back(twist.rot.y() * rotational_scale);
+                inputs.push_back(twist.rot.z() * rotational_scale);
+            }
+        }
+
+        for(auto& v : inputs)
+            if(!std::isfinite(v)) throw std::runtime_error("NAN");
+        for(auto& v : outputs)
+            if(!std::isfinite(v)) throw std::runtime_error("NAN");
+
+        input_minmax.resize(input_count);
+        output_minmax.resize(output_count);
+
+        find_minmax(inputs, input_minmax);
+        find_minmax(outputs, output_minmax);
+
+        normalize(inputs, input_minmax);
+        normalize(outputs, output_minmax);
+
+        for(size_t iter = 0; iter < samples; iter++)
+        {
+            input_pp.push_back(inputs.data() + iter * input_count);
+            output_pp.push_back(outputs.data() + iter * output_count);
+        }
+
+        LOG("neuro ik training");
+
+        FANN::training_data train;
+        train.set_train_data(samples, input_count, input_pp.data(), output_count, output_pp.data());
+        net.set_callback(
+            [](FANN::neural_net& net, FANN::training_data& train, unsigned int max_epochs, unsigned int epochs_between_reports, float desired_error, unsigned int epochs, void* user_data) {
+                if(epochs % epochs_between_reports != 0) return 0;
+                // LOG("training", epochs, "/", max_epochs, epochs * 100 / max_epochs, "%");
+                LOG("training", epochs, net.get_MSE(), desired_error);
+                return 0;
+            },
+            0);
+
+        net.set_activation_function_hidden(FANN::SIGMOID);
+        net.set_activation_function_output(FANN::SIGMOID);
+
+        net.init_weights(train);
+
+        net.train_on_data(train, 1000, 1, 0.0001);
+
+        fann_type err = net.test_data(train);
+        LOG("neuro ik training error:", err);
+
+        /*std::vector<fann_type> iiv, oov, ttv;
+        for(size_t iter = 0; iter < 10; iter++)
+        {
+            auto* ii = input_pp[iter];
+            auto* oo = net.run(ii);
+            auto* tt = output_pp[iter];
+            iiv.assign(ii, ii + input_count);
+            ttv.assign(tt, tt + output_count);
+            oov.assign(oo, oo + output_count);
+            LOG_LIST(iiv);
+            LOG_LIST(ttv);
+            LOG_LIST(oov);
+        }*/
+
+        LOG("training done");
+    }
+
+    size_t iterations = 0;
+
+    void initialize(const Problem& problem)
+    {
+        static std::mutex mutex;
+        std::lock_guard<std::mutex> lock(mutex);
+        IKBase::initialize(problem);
+        solution = problem.initial_guess;
+        if(!trained) train();
+        iterations = 0;
+        if(thread_index > 0)
+            for(auto& vi : problem.active_variables)
+                solution[vi] = random(modelInfo.getMin(vi), modelInfo.getMax(vi));
+    }
+
+    const std::vector<double>& getSolution() const { return solution; }
+
+    std::vector<fann_type> inputs, outputs;
+
+    std::vector<Frame> tip_objectives;
+
+    /*void step()
+    {
+        //if(iterations > 1) return;
+        iterations++;
+
+        inputs.clear();
+        for(auto ivar : problem.active_variables)
+        {
+            inputs.push_back(solution[ivar]);
+        }
+
+        tip_objectives.resize(model.getTipFrames().size());
+        for(auto& g : problem.goals)
+        {
+            tip_objectives[g.tip_index] = g.frame;
+        }
+
+        model.applyConfiguration(solution);
+        auto& frames1 = model.getTipFrames();
+        auto& frames2 = tip_objectives;
+
+        double scale = 1.0;
+
+        for(size_t itip = 0; itip < tip_objectives.size(); itip++)
+        {
+            double translational_scale = 1.0;
+            double rotational_scale = 1.0;
+
+            //Frame diff = inverse(frames1[itip]) * frames2[itip];
+            //auto twist = toTwist(diff);
+            auto twist = frameTwist(frames1[itip], frames2[itip]);
+
+            auto dpos = frames2[itip].pos - frames1[itip].pos;
+            auto drot = Vector3(
+                twist.rot.x() * rotational_scale,
+                twist.rot.y() * rotational_scale,
+                twist.rot.z() * rotational_scale
+            );
+
+            scale = 1.0 / (0.0000001 + dpos.length() + drot.length());
+
+            dpos = dpos * scale;
+            drot = drot * scale;
+
+            inputs.push_back(dpos.x());
+            inputs.push_back(dpos.y());
+            inputs.push_back(dpos.z());
+
+            inputs.push_back(drot.x());
+            inputs.push_back(drot.y());
+            inputs.push_back(drot.z());
+        }
+
+        normalize(inputs, input_minmax);
+
+        auto* oo = net.run(inputs.data());
+
+        outputs.assign(oo, oo + output_count);
+
+        denormalize(outputs, output_minmax);
+
+        auto& vv = problem.active_variables;
+        for(size_t iout = 0; iout < vv.size(); iout++)
+        {
+            size_t ivar = vv[iout];
+            solution[ivar] = modelInfo.clip(solution[ivar] + outputs[iout] * 0.1 / scale, ivar);
+        }
+    }*/
+
+    void step()
+    {
+        // if(iterations > 1) return;
+        iterations++;
+
+        inputs.clear();
+        for(auto ivar : problem.active_variables)
+        {
+            inputs.push_back(solution[ivar]);
+        }
+
+        tip_objectives.resize(model.getTipFrames().size());
+        for(auto& g : problem.goals)
+        {
+            tip_objectives[g.tip_index] = g.frame;
+        }
+
+        model.applyConfiguration(solution);
+        auto& frames1 = model.getTipFrames();
+        auto& frames2 = tip_objectives;
+
+        double scale = 1.0;
+        for(size_t itip = 0; itip < tip_objectives.size(); itip++)
+        {
+            double translational_scale = 1.0;
+            double rotational_scale = 1.0;
+            auto twist = frameTwist(frames1[itip], frames2[itip]);
+            auto dpos = frames2[itip].pos - frames1[itip].pos;
+            auto drot = Vector3(twist.rot.x() * rotational_scale, twist.rot.y() * rotational_scale, twist.rot.z() * rotational_scale);
+
+            /*if(iterations % 2)
+            {
+                scale = 1.0 / (0.0000001 + dpos.length());
+                inputs.push_back(dpos.x() * scale);
+                inputs.push_back(dpos.y() * scale);
+                inputs.push_back(dpos.z() * scale);
+                inputs.push_back(0);
+                inputs.push_back(0);
+                inputs.push_back(0);
+            } else {
+                scale = 1.0 / (0.0000001 + drot.length());
+                inputs.push_back(0);
+                inputs.push_back(0);
+                inputs.push_back(0);
+                inputs.push_back(drot.x() * scale);
+                inputs.push_back(drot.y() * scale);
+                inputs.push_back(drot.z() * scale);
+            }*/
+
+            {
+                scale = 1.0 / (0.0000001 + dpos.length() + drot.length());
+                inputs.push_back(dpos.x() * scale);
+                inputs.push_back(dpos.y() * scale);
+                inputs.push_back(dpos.z() * scale);
+                inputs.push_back(drot.x() * scale);
+                inputs.push_back(drot.y() * scale);
+                inputs.push_back(drot.z() * scale);
+            }
+        }
+        normalize(inputs, input_minmax);
+        auto* oo = net.run(inputs.data());
+        outputs.assign(oo, oo + output_count);
+        denormalize(outputs, output_minmax);
+        auto& vv = problem.active_variables;
+        for(size_t iout = 0; iout < vv.size(); iout++)
+        {
+            size_t ivar = vv[iout];
+            solution[ivar] = modelInfo.clip(solution[ivar] + outputs[iout] * 1 / scale, ivar);
+        }
+    }
+};
+
+static IKFactory::Class<IKNeural> neural("neural");
+
+struct IKNeural2 : IKBase
+{
+    std::vector<double> solution;
+    FANN::neural_net net;
+
+    std::vector<std::pair<fann_type, fann_type>> input_minmax, output_minmax;
+
+    /*static void find_minmax(const std::vector<fann_type>& values, std::vector<std::pair<fann_type, fann_type>>& minmax)
+    {
+        for(size_t i = 0; i < minmax.size(); i++)
+        {
+            minmax[i] = std::make_pair(values[i], values[i]);
+        }
+        for(size_t i = 0; i < values.size(); i++)
+        {
+            auto& v = values[i];
+            auto& p = minmax[i % minmax.size()];
+            p.first = std::min(p.first, v);
+            p.second = std::max(p.second, v);
+        }
+    }*/
+
+    static void find_minmax(const std::vector<fann_type>& values, std::vector<std::pair<fann_type, fann_type>>& minmax)
+    {
+        std::vector<double> centers(minmax.size(), 0.0);
+        for(size_t i = 0; i < values.size(); i++)
+            centers[i % minmax.size()] += values[i] * (1.0 * minmax.size() / values.size());
+
+        std::vector<double> ranges2(minmax.size(), 0.0001);
+        for(size_t i = 0; i < values.size(); i++)
+        {
+            double d = values[i] - centers[i % minmax.size()];
+            d = d * d;
+            ranges2[i % minmax.size()] += d * (1.0 * minmax.size() / values.size());
+        }
+
+        for(size_t i = 0; i < minmax.size(); i++)
+        {
+            auto& p = minmax[i];
+            p.first = centers[i] - sqrt(ranges2[i]);
+            p.second = centers[i] + sqrt(ranges2[i]);
+        }
+    }
+
+    static void normalize(std::vector<fann_type>& values, const std::vector<std::pair<fann_type, fann_type>>& minmax)
+    {
+        for(size_t i = 0; i < values.size(); i++)
+        {
+            auto& v = values[i];
+            auto& p = minmax[i % minmax.size()];
+            v = (v - p.first) / (p.second - p.first);
+        }
+    }
+
+    static void denormalize(std::vector<fann_type>& values, const std::vector<std::pair<fann_type, fann_type>>& minmax)
+    {
+        for(size_t i = 0; i < values.size(); i++)
+        {
+            auto& v = values[i];
+            auto& p = minmax[i % minmax.size()];
+            v = v * (p.second - p.first) + p.first;
+        }
+    }
+
+    unsigned int input_count, output_count;
+
+    IKNeural2(const IKParams& p)
+        : IKBase(p)
+    {
+        trained = false;
+    }
+
+    bool trained;
+
+    void train()
+    {
+        trained = true;
+
+        input_count = problem.tip_link_indices.size() * 7;
+        output_count = problem.active_variables.size();
+
+        LOG_VAR(input_count);
+        LOG_VAR(output_count);
+
+        // std::vector<unsigned int> levels = {input_count, 100, 100, output_count};
+
+        // std::vector<unsigned int> levels = {input_count, input_count, input_count, output_count};
+
+        std::vector<unsigned int> levels = {input_count, input_count + output_count, output_count};
+
+        // std::vector<unsigned int> levels = {input_count, input_count + output_count, input_count + output_count, output_count};
+
+        // std::vector<unsigned int> levels = {input_count, output_count};
+
+        net.create_standard_array(levels.size(), levels.data());
+
+        size_t var_count = params.robot_model->getVariableCount();
+        std::vector<double> state = problem.initial_guess;
+        std::vector<Frame> frames;
+
+        std::vector<fann_type> inputs, outputs;
+        std::vector<fann_type*> input_pp, output_pp;
+
+        LOG("neuro ik generating training data");
+
+        unsigned int samples = 10000;
+
+        for(size_t iter = 0; iter < samples; iter++)
+        {
+            for(size_t ivar : problem.active_variables)
+                state[ivar] = random(modelInfo.getMin(ivar), modelInfo.getMax(ivar));
+
+            model.applyConfiguration(state);
+            frames = model.getTipFrames();
+
+            for(auto ivar : problem.active_variables)
+                outputs.push_back(state[ivar]);
+
+            for(size_t itip = 0; itip < problem.tip_link_indices.size(); itip++)
+            {
+                inputs.push_back(frames[itip].pos.x());
+                inputs.push_back(frames[itip].pos.y());
+                inputs.push_back(frames[itip].pos.z());
+
+                auto rot = frames[itip].rot;
+                rot = rot * rot;
+                // rot = tf::Quaternion(0, 0, 0, 1);
+                inputs.push_back(rot.x());
+                inputs.push_back(rot.y());
+                inputs.push_back(rot.z());
+                inputs.push_back(rot.w());
+            }
+        }
+
+        for(auto& v : inputs)
+            if(!std::isfinite(v)) throw std::runtime_error("NAN");
+        for(auto& v : outputs)
+            if(!std::isfinite(v)) throw std::runtime_error("NAN");
+
+        input_minmax.resize(input_count);
+        output_minmax.resize(output_count);
+
+        find_minmax(inputs, input_minmax);
+        find_minmax(outputs, output_minmax);
+
+        normalize(inputs, input_minmax);
+        normalize(outputs, output_minmax);
+
+        for(size_t iter = 0; iter < samples; iter++)
+        {
+            input_pp.push_back(inputs.data() + iter * input_count);
+            output_pp.push_back(outputs.data() + iter * output_count);
+        }
+
+        LOG("neuro ik training");
+
+        FANN::training_data train;
+        train.set_train_data(samples, input_count, input_pp.data(), output_count, output_pp.data());
+        net.set_callback(
+            [](FANN::neural_net& net, FANN::training_data& train, unsigned int max_epochs, unsigned int epochs_between_reports, float desired_error, unsigned int epochs, void* user_data) {
+                if(epochs % epochs_between_reports != 0) return 0;
+                // LOG("training", epochs, "/", max_epochs, epochs * 100 / max_epochs, "%");
+                LOG("training", epochs, net.get_MSE(), desired_error);
+                return 0;
+            },
+            0);
+
+        net.set_activation_function_hidden(FANN::SIGMOID);
+        net.set_activation_function_output(FANN::SIGMOID);
+
+        net.init_weights(train);
+
+        net.train_on_data(train, 100, 1, 0.0001);
+
+        fann_type err = net.test_data(train);
+        LOG("neuro ik training error:", err);
+
+        /*std::vector<fann_type> iiv, oov, ttv;
+        for(size_t iter = 0; iter < 10; iter++)
+        {
+            auto* ii = input_pp[iter];
+            auto* oo = net.run(ii);
+            auto* tt = output_pp[iter];
+            iiv.assign(ii, ii + input_count);
+            ttv.assign(tt, tt + output_count);
+            oov.assign(oo, oo + output_count);
+            LOG_LIST(iiv);
+            LOG_LIST(ttv);
+            LOG_LIST(oov);
+        }*/
+
+        LOG("training done");
+    }
+
+    size_t iterations = 0;
+
+    void initialize(const Problem& problem)
+    {
+        IKBase::initialize(problem);
+        solution = problem.initial_guess;
+        if(!trained) train();
+        iterations = 0;
+    }
+
+    const std::vector<double>& getSolution() const { return solution; }
+
+    std::vector<fann_type> inputs, outputs;
+
+    std::vector<Frame> tip_objectives;
+
+    void step()
+    {
+        if(iterations > 1) return;
+        iterations++;
+
+        inputs.clear();
+
+        tip_objectives.resize(model.getTipFrames().size());
+        for(auto& g : problem.goals)
+        {
+            tip_objectives[g.tip_index] = g.frame;
+        }
+
+        auto& frames = tip_objectives;
+
+        for(size_t itip = 0; itip < tip_objectives.size(); itip++)
+        {
+            inputs.push_back(frames[itip].pos.x());
+            inputs.push_back(frames[itip].pos.y());
+            inputs.push_back(frames[itip].pos.z());
+
+            auto rot = frames[itip].rot;
+            rot = rot * rot;
+            // rot = tf::Quaternion(0, 0, 0, 1);
+            inputs.push_back(rot.x());
+            inputs.push_back(rot.y());
+            inputs.push_back(rot.z());
+            inputs.push_back(rot.w());
+        }
+
+        normalize(inputs, input_minmax);
+
+        auto* oo = net.run(inputs.data());
+
+        outputs.assign(oo, oo + output_count);
+
+        denormalize(outputs, output_minmax);
+
+        auto& vv = problem.active_variables;
+        for(size_t iout = 0; iout < vv.size(); iout++)
+        {
+            size_t ivar = vv[iout];
+            solution[ivar] = modelInfo.clip(outputs[iout], ivar);
+        }
+    }
+};
+
+static IKFactory::Class<IKNeural2> neural2("neural2");
+}

src/bio_ik-kinetic-support/src/ik_parallel.h

@@ -0,0 +1,278 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016-2017, Philipp Sebastian Ruppel
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include "ik_base.h"
+
+#include <boost/thread/barrier.hpp>
+
+namespace bio_ik
+{
+
+// executes a function in parallel on pre-allocated threads
+class ParallelExecutor
+{
+    std::function<void(size_t)> fun;
+    std::vector<std::thread> threads;
+    boost::barrier barrier;
+    volatile bool exit;
+    double best_fitness;
+
+public:
+    template <class FUN>
+    ParallelExecutor(size_t thread_count, const FUN& f)
+        : exit(false)
+        , threads(thread_count)
+        , fun(f)
+        , barrier(thread_count)
+    {
+        for(size_t i = 1; i < thread_count; i++)
+        {
+            std::thread t([this, i]() {
+                while(true)
+                {
+                    barrier.wait();
+                    if(exit) break;
+                    fun(i);
+                    barrier.wait();
+                    if(exit) break;
+                }
+            });
+            std::swap(t, threads[i]);
+        }
+    }
+    ~ParallelExecutor()
+    {
+        exit = true;
+        barrier.wait();
+        for(auto& t : threads)
+            if(t.joinable()) t.join();
+    }
+    void run()
+    {
+        barrier.wait();
+        fun(0);
+        barrier.wait();
+    }
+};
+
+// runs ik on multiple threads until a stop criterion is met
+struct IKParallel
+{
+    IKParams params;
+    std::vector<std::unique_ptr<IKSolver>> solvers;
+    std::vector<std::vector<double>> solver_solutions;
+    std::vector<std::vector<double>> solver_temps;
+    std::vector<int> solver_success;
+    std::vector<double> solver_fitness;
+    int thread_count;
+    // std::vector<RobotFK_Fast> fk; // TODO: remove
+    double timeout;
+    bool success;
+    std::atomic<int> finished;
+    std::atomic<uint32_t> iteration_count;
+    std::vector<double> result;
+    std::unique_ptr<ParallelExecutor> par;
+    Problem problem;
+    bool enable_counter;
+    double best_fitness;
+
+    IKParallel(const IKParams& params)
+        : params(params)
+    {
+        // solver class name
+        std::string name = params.solver_class_name;
+
+        enable_counter = params.enable_counter;
+
+        // create solvers
+        solvers.emplace_back(IKFactory::create(name, params));
+        thread_count = solvers.front()->concurrency();
+        if(params.thread_count) {
+            thread_count = params.thread_count;
+        }
+        while(solvers.size() < thread_count)
+            solvers.emplace_back(IKFactory::clone(solvers.front().get()));
+        for(size_t i = 0; i < thread_count; i++)
+            solvers[i]->thread_index = i;
+
+        // while(fk.size() < thread_count) fk.emplace_back(params.robot_model);
+
+        // init buffers
+        solver_solutions.resize(thread_count);
+        solver_temps.resize(thread_count);
+        solver_success.resize(thread_count);
+        solver_fitness.resize(thread_count);
+
+        // create parallel executor
+        par.reset(new ParallelExecutor(thread_count, [this](size_t i) { solverthread(i); }));
+    }
+
+    void initialize(const Problem& problem)
+    {
+        this->problem = problem;
+        // for(auto& f : fk) f.initialize(problem.tip_link_indices);
+    }
+
+private:
+    void solverthread(size_t i)
+    {
+        THREADPROFILER("thread", i);
+        COUNTERPROFILER("solver threads");
+
+        // initialize ik solvers
+        {
+            BLOCKPROFILER("ik solver init");
+            solvers[i]->initialize(problem);
+        }
+
+        // run solver iterations until solution found or timeout
+        for(size_t iteration = 0; (ros::WallTime::now().toSec() < timeout && finished == 0) || (iteration == 0 && i == 0); iteration++)
+        {
+            if(finished) break;
+
+            // run solver for a few steps
+            solvers[i]->step();
+            iteration_count++;
+            for(int it2 = 1; it2 < 4; it2++)
+                if(ros::WallTime::now().toSec() < timeout && finished == 0) solvers[i]->step();
+
+            if(finished) break;
+
+            // get solution and check stop criterion
+            auto& result = solver_temps[i];
+            result = solvers[i]->getSolution();
+            auto& fk = solvers[i]->model;
+            fk.applyConfiguration(result);
+            bool success = solvers[i]->checkSolution(result, fk.getTipFrames());
+            if(success) finished = 1;
+            solver_success[i] = success;
+            solver_solutions[i] = result;
+            solver_fitness[i] = solvers[i]->computeFitness(result, fk.getTipFrames());
+
+            if(success) break;
+        }
+
+        finished = 1;
+
+        for(auto& s : solvers)
+            s->canceled = true;
+    }
+
+public:
+    void solve()
+    {
+        BLOCKPROFILER("solve mt");
+
+        // prepare
+        iteration_count = 0;
+        result = problem.initial_guess;
+        timeout = problem.timeout;
+        success = false;
+        finished = 0;
+        for(auto& s : solver_solutions)
+            s = problem.initial_guess;
+        for(auto& s : solver_temps)
+            s = problem.initial_guess;
+        for(auto& s : solver_success)
+            s = 0;
+        for(auto& f : solver_fitness)
+            f = DBL_MAX;
+        for(auto& s : solvers)
+            s->canceled = false;
+
+        // run solvers
+        {
+            BLOCKPROFILER("solve mt 2");
+            par->run();
+        }
+
+        size_t best_index = 0;
+        best_fitness = DBL_MAX;
+
+        // if exact primary goal matches have been found ...
+        for(size_t i = 0; i < thread_count; i++)
+        {
+            if(solver_success[i])
+            {
+                double fitness;
+                if(solvers[0]->problem.secondary_goals.empty())
+                {
+                    // ... and if no secondary goals have been specified,
+                    // select the best result according to primary goals
+                    fitness = solver_fitness[i];
+                }
+                else
+                {
+                    // ... and if secondary goals have been specified,
+                    // select the result that best satisfies primary and secondary goals
+                    fitness = solver_fitness[i] + solvers[0]->computeSecondaryFitnessAllVariables(solver_solutions[i]);
+                }
+                if(fitness < best_fitness)
+                {
+                    best_fitness = fitness;
+                    best_index = i;
+                }
+            }
+        }
+
+        // if no exact primary goal matches have been found,
+        // select best primary goal approximation
+        if(best_fitness == DBL_MAX)
+        {
+            for(size_t i = 0; i < thread_count; i++)
+            {
+                if(solver_fitness[i] < best_fitness)
+                {
+                    best_fitness = solver_fitness[i];
+                    best_index = i;
+                }
+            }
+        }
+
+        if(enable_counter)
+        {
+            LOG("iterations", iteration_count);
+        }
+
+        result = solver_solutions[best_index];
+        success = solver_success[best_index];
+    }
+
+    double getSolutionFitness() const { return best_fitness; }
+
+    bool getSuccess() const { return success; }
+
+    const std::vector<double>& getSolution() const { return result; }
+};
+}

src/bio_ik-kinetic-support/src/ik_test.cpp

@@ -0,0 +1,137 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016-2017, Philipp Sebastian Ruppel
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include "ik_base.h"
+
+namespace bio_ik
+{
+
+struct IKTest : IKBase
+{
+
+    RobotFK_MoveIt fkref;
+
+    std::vector<double> temp;
+
+    double d_rot_sum, d_pos_sum, d_div;
+
+    IKTest(const IKParams& params)
+        : IKBase(params)
+        , fkref(params.robot_model)
+    {
+        d_rot_sum = d_pos_sum = d_div = 0;
+    }
+
+    /*double tipdiff(const std::vector<Frame>& fa, const std::vector<Frame>& fb)
+    {
+        double diff = 0.0;
+        for(size_t i = 0; i < problem.tip_link_indices.size(); i++)
+        {
+            //LOG_VAR(fa[i]);
+            //LOG_VAR(fb[i]);
+            diff += fa[i].rot.angleShortestPath(fb[i].rot);
+            diff += fa[i].pos.distance(fb[i].pos);
+        }
+        return diff;
+    }*/
+
+    void initialize(const Problem& problem)
+    {
+        IKBase::initialize(problem);
+
+        fkref.initialize(problem.tip_link_indices);
+        model.initialize(problem.tip_link_indices);
+
+        fkref.applyConfiguration(problem.initial_guess);
+        model.applyConfiguration(problem.initial_guess);
+
+        // double diff = tipdiff(fkref.getTipFrames(), model.getTipFrames());
+        // LOG_VAR(diff);
+
+        /*{
+            auto& fa = fkref.getTipFrames();
+            auto& fb = model.getTipFrames();
+            for(size_t i = 0; i < problem.tip_link_indices.size(); i++)
+            {
+                LOG("d rot", i, fa[i].rot.angleShortestPath(fb[i].rot));
+                LOG("d pos", i, fa[i].pos.distance(fb[i].pos));
+            }
+        }*/
+
+        {
+            temp = problem.initial_guess;
+            for(size_t ivar : problem.active_variables)
+                if(modelInfo.isRevolute(ivar) || modelInfo.isPrismatic(ivar)) temp[ivar] = modelInfo.clip(temp[ivar] + random(-0.1, 0.1), ivar);
+
+            fkref.applyConfiguration(temp);
+            auto& fa = fkref.getTipFrames();
+
+            model.applyConfiguration(problem.initial_guess);
+            model.initializeMutationApproximator(problem.active_variables);
+
+            std::vector<aligned_vector<Frame>> fbm;
+
+            std::vector<double> mutation_values;
+            for(size_t ivar : problem.active_variables)
+                mutation_values.push_back(temp[ivar]);
+            const double* mutation_ptr = mutation_values.data();
+
+            model.computeApproximateMutations(1, &mutation_ptr, fbm);
+
+            auto& fb = fbm[0];
+
+            // auto& fb = model.getTipFrames();
+
+            for(size_t i = 0; i < problem.tip_link_indices.size(); i++)
+            {
+                // LOG("d rot", i, fa[i].rot.angleShortestPath(fb[i].rot));
+                // LOG("d pos", i, fa[i].pos.distance(fb[i].pos));
+
+                d_rot_sum += fa[i].rot.angleShortestPath(fb[i].rot);
+                d_pos_sum += fa[i].pos.distance(fb[i].pos);
+                d_div += 1;
+            }
+        }
+
+        LOG("d rot avg", d_rot_sum / d_div);
+        LOG("d pos avg", d_pos_sum / d_div);
+    }
+
+    void step() {}
+
+    const std::vector<double>& getSolution() const { return problem.initial_guess; }
+};
+
+static IKFactory::Class<IKTest> test("test");
+}

src/bio_ik-kinetic-support/src/kinematics_plugin.cpp

@@ -0,0 +1,649 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016-2017, Philipp Sebastian Ruppel
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include <bio_ik/goal.h>
+
+#include "forward_kinematics.h"
+#include "ik_base.h"
+#include "ik_parallel.h"
+#include "problem.h"
+#include "utils.h"
+
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+#include <kdl_parser/kdl_parser.hpp>
+#include <moveit/kinematics_base/kinematics_base.h>
+#include <moveit/rdf_loader/rdf_loader.h>
+#include <pluginlib/class_list_macros.h>
+#include <srdfdom/model.h>
+#include <urdf/model.h>
+#include <urdf_model/model.h>
+
+#include <eigen_conversions/eigen_msg.h>
+//#include <moveit/common_planning_interface_objects/common_objects.h>
+#include <moveit/kinematics_base/kinematics_base.h>
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_state/robot_state.h>
+
+#include <atomic>
+#include <mutex>
+#include <random>
+#include <tuple>
+#include <type_traits>
+
+#include <bio_ik/goal_types.h>
+
+using namespace bio_ik;
+
+// implement BioIKKinematicsQueryOptions
+
+namespace bio_ik {
+
+std::mutex bioIKKinematicsQueryOptionsMutex;
+std::unordered_set<const void *> bioIKKinematicsQueryOptionsList;
+
+BioIKKinematicsQueryOptions::BioIKKinematicsQueryOptions()
+    : replace(false), solution_fitness(0) {
+  std::lock_guard<std::mutex> lock(bioIKKinematicsQueryOptionsMutex);
+  bioIKKinematicsQueryOptionsList.insert(this);
+}
+
+BioIKKinematicsQueryOptions::~BioIKKinematicsQueryOptions() {
+  std::lock_guard<std::mutex> lock(bioIKKinematicsQueryOptionsMutex);
+  bioIKKinematicsQueryOptionsList.erase(this);
+}
+
+bool isBioIKKinematicsQueryOptions(const void *ptr) {
+  std::lock_guard<std::mutex> lock(bioIKKinematicsQueryOptionsMutex);
+  return bioIKKinematicsQueryOptionsList.find(ptr) !=
+         bioIKKinematicsQueryOptionsList.end();
+}
+
+const BioIKKinematicsQueryOptions *
+toBioIKKinematicsQueryOptions(const void *ptr) {
+  if (isBioIKKinematicsQueryOptions(ptr))
+    return (const BioIKKinematicsQueryOptions *)ptr;
+  else
+    return 0;
+}
+
+} // namespace bio_ik
+
+// BioIK Kinematics Plugin
+
+namespace bio_ik_kinematics_plugin {
+
+// Fallback for older MoveIt versions which don't support lookupParam yet
+template <class T>
+static void lookupParam(const std::string &param, T &val,
+                        const T &default_val) {
+  ros::NodeHandle nodeHandle("~");
+  val = nodeHandle.param(param, default_val);
+}
+
+struct BioIKKinematicsPlugin : kinematics::KinematicsBase {
+  std::vector<std::string> joint_names, link_names;
+  moveit::core::RobotModelConstPtr robot_model;
+  const moveit::core::JointModelGroup *joint_model_group;
+  mutable std::unique_ptr<IKParallel> ik;
+  mutable std::vector<double> state, temp;
+  mutable std::unique_ptr<moveit::core::RobotState> temp_state;
+  mutable std::vector<Frame> tipFrames;
+  RobotInfo robot_info;
+  bool enable_profiler;
+
+  BioIKKinematicsPlugin() { enable_profiler = false; }
+
+  virtual const std::vector<std::string> &getJointNames() const {
+    LOG_FNC();
+    return joint_names;
+  }
+
+  virtual const std::vector<std::string> &getLinkNames() const {
+    LOG_FNC();
+    return link_names;
+  }
+
+  virtual bool getPositionFK(const std::vector<std::string> &link_names,
+                             const std::vector<double> &joint_angles,
+                             std::vector<geometry_msgs::Pose> &poses) const {
+    LOG_FNC();
+    return false;
+  }
+
+  virtual bool getPositionIK(const geometry_msgs::Pose &ik_pose,
+                             const std::vector<double> &ik_seed_state,
+                             std::vector<double> &solution,
+                             moveit_msgs::MoveItErrorCodes &error_code,
+                             const kinematics::KinematicsQueryOptions &options =
+                                 kinematics::KinematicsQueryOptions()) const {
+    LOG_FNC();
+    return false;
+  }
+
+  std::vector<Eigen::Affine3d> tip_reference_frames;
+
+  mutable std::vector<std::unique_ptr<Goal>> default_goals;
+
+  mutable std::vector<const bio_ik::Goal *> all_goals;
+
+  IKParams ikparams;
+
+  mutable Problem problem;
+
+  static moveit::core::RobotModelConstPtr
+  loadRobotModel(const std::string &robot_description) {
+    static std::map<std::string, moveit::core::RobotModelConstPtr>
+        robot_model_cache;
+    static std::mutex cache_mutex;
+    std::lock_guard<std::mutex> lock(cache_mutex);
+    if (robot_model_cache.find(robot_description) == robot_model_cache.end()) {
+      rdf_loader::RDFLoader rdf_loader(robot_description);
+      auto srdf = rdf_loader.getSRDF();
+      auto urdf_model = rdf_loader.getURDF();
+
+      if (!urdf_model || !srdf) {
+        LOG("URDF and SRDF must be loaded for kinematics solver to work.");
+        return nullptr;
+      }
+      robot_model_cache[robot_description] = moveit::core::RobotModelConstPtr(
+          new robot_model::RobotModel(urdf_model, srdf));
+    }
+    return robot_model_cache[robot_description];
+
+    // return
+    // moveit::planning_interface::getSharedRobotModel(robot_description);
+  }
+
+  bool load(std::string robot_description, std::string group_name) {
+    LOG_FNC();
+
+    // LOG_VAR(robot_description);
+    // LOG_VAR(group_name);
+
+    LOG("bio ik init", ros::this_node::getName());
+
+    /*rdf_loader::RDFLoader rdf_loader(robot_description_);
+    auto srdf = rdf_loader.getSRDF();
+    auto urdf_model = rdf_loader.getURDF();
+
+    if(!urdf_model || !srdf)
+    {
+        LOG("URDF and SRDF must be loaded for kinematics solver to work.");
+        return false;
+    }
+
+    robot_model.reset(new robot_model::RobotModel(urdf_model, srdf));*/
+
+    robot_model = loadRobotModel(robot_description);
+
+    joint_model_group = robot_model->getJointModelGroup(group_name);
+    if (!joint_model_group) {
+      LOG("failed to get joint model group");
+      return false;
+    }
+
+    joint_names.clear();
+
+    for (auto *joint_model : joint_model_group->getJointModels())
+      if (joint_model->getName() != base_frame_ &&
+          joint_model->getType() != moveit::core::JointModel::UNKNOWN &&
+          joint_model->getType() != moveit::core::JointModel::FIXED)
+        joint_names.push_back(joint_model->getName());
+
+    auto tips2 = tip_frames_;
+    joint_model_group->getEndEffectorTips(tips2);
+    if (!tips2.empty())
+      tip_frames_ = tips2;
+
+    link_names = tip_frames_;
+
+    // for(auto& n : joint_names) LOG("joint", n);
+    // for(auto& n : link_names) LOG("link", n);
+
+    // bool enable_profiler;
+    lookupParam("profiler", enable_profiler, false);
+    // if(enable_profiler) Profiler::start();
+
+    robot_info = RobotInfo(robot_model);
+
+    ikparams.robot_model = robot_model;
+    ikparams.joint_model_group = joint_model_group;
+
+    // initialize parameters for IKParallel
+    lookupParam("mode", ikparams.solver_class_name,
+                std::string("bio2_memetic"));
+    lookupParam("counter", ikparams.enable_counter, false);
+    lookupParam("threads", ikparams.thread_count, 0);
+
+    // initialize parameters for Problem
+    lookupParam("dpos", ikparams.dpos, DBL_MAX);
+    lookupParam("drot", ikparams.drot, DBL_MAX);
+    lookupParam("dtwist", ikparams.dtwist, 1e-5);
+
+    // initialize parameters for ik_evolution_1
+    lookupParam("no_wipeout", ikparams.opt_no_wipeout, false);
+    lookupParam("population_size", ikparams.population_size, 8);
+    lookupParam("elite_count", ikparams.elite_count, 4);
+    lookupParam("linear_fitness", ikparams.linear_fitness, false);
+
+    temp_state.reset(new moveit::core::RobotState(robot_model));
+
+    ik.reset(new IKParallel(ikparams));
+
+    {
+
+      BLOCKPROFILER("default ik goals");
+
+      default_goals.clear();
+
+      for (size_t i = 0; i < tip_frames_.size(); i++) {
+        PoseGoal *goal = new PoseGoal();
+
+        goal->setLinkName(tip_frames_[i]);
+
+        // LOG_VAR(goal->link_name);
+
+        double rotation_scale = 0.5;
+
+        lookupParam("rotation_scale", rotation_scale, rotation_scale);
+
+        bool position_only_ik = false;
+        lookupParam("position_only_ik", position_only_ik, position_only_ik);
+        if (position_only_ik)
+          rotation_scale = 0;
+
+        goal->setRotationScale(rotation_scale);
+
+        default_goals.emplace_back(goal);
+      }
+
+      {
+        double weight = 0;
+        lookupParam("center_joints_weight", weight, weight);
+        if (weight > 0.0) {
+          auto *avoid_joint_limits_goal = new bio_ik::CenterJointsGoal();
+          avoid_joint_limits_goal->setWeight(weight);
+          default_goals.emplace_back(avoid_joint_limits_goal);
+        }
+      }
+
+      {
+        double weight = 0;
+        lookupParam("avoid_joint_limits_weight", weight, weight);
+        if (weight > 0.0) {
+          auto *avoid_joint_limits_goal = new bio_ik::AvoidJointLimitsGoal();
+          avoid_joint_limits_goal->setWeight(weight);
+          default_goals.emplace_back(avoid_joint_limits_goal);
+        }
+      }
+
+      {
+        double weight = 0;
+        lookupParam("minimal_displacement_weight", weight, weight);
+        if (weight > 0.0) {
+          auto *minimal_displacement_goal =
+              new bio_ik::MinimalDisplacementGoal();
+          minimal_displacement_goal->setWeight(weight);
+          default_goals.emplace_back(minimal_displacement_goal);
+        }
+      }
+    }
+
+    // LOG("init ready");
+
+    return true;
+  }
+
+  virtual bool initialize(const std::string &robot_description,
+                          const std::string &group_name,
+                          const std::string &base_frame,
+                          const std::string &tip_frame,
+                          double search_discretization) {
+    LOG_FNC();
+    std::vector<std::string> tip_frames;
+    tip_frames.push_back(tip_frame);
+    initialize(robot_description, group_name, base_frame, tip_frames,
+               search_discretization);
+    return true;
+  }
+
+  virtual bool initialize(const std::string &robot_description,
+                          const std::string &group_name,
+                          const std::string &base_frame,
+                          const std::vector<std::string> &tip_frames,
+                          double search_discretization) {
+    LOG_FNC();
+    setValues(robot_description, group_name, base_frame, tip_frames,
+              search_discretization);
+    load(robot_description, group_name);
+    return true;
+  }
+
+  virtual bool
+  searchPositionIK(const geometry_msgs::Pose &ik_pose,
+                   const std::vector<double> &ik_seed_state, double timeout,
+                   std::vector<double> &solution,
+                   moveit_msgs::MoveItErrorCodes &error_code,
+                   const kinematics::KinematicsQueryOptions &options =
+                       kinematics::KinematicsQueryOptions()) const {
+    LOG_FNC();
+    return searchPositionIK(std::vector<geometry_msgs::Pose>{ik_pose},
+                            ik_seed_state, timeout, std::vector<double>(),
+                            solution, IKCallbackFn(), error_code, options);
+  }
+
+  virtual bool
+  searchPositionIK(const geometry_msgs::Pose &ik_pose,
+                   const std::vector<double> &ik_seed_state, double timeout,
+                   const std::vector<double> &consistency_limits,
+                   std::vector<double> &solution,
+                   moveit_msgs::MoveItErrorCodes &error_code,
+                   const kinematics::KinematicsQueryOptions &options =
+                       kinematics::KinematicsQueryOptions()) const {
+    LOG_FNC();
+    return searchPositionIK(std::vector<geometry_msgs::Pose>{ik_pose},
+                            ik_seed_state, timeout, consistency_limits,
+                            solution, IKCallbackFn(), error_code, options);
+  }
+
+  virtual bool
+  searchPositionIK(const geometry_msgs::Pose &ik_pose,
+                   const std::vector<double> &ik_seed_state, double timeout,
+                   std::vector<double> &solution,
+                   const IKCallbackFn &solution_callback,
+                   moveit_msgs::MoveItErrorCodes &error_code,
+                   const kinematics::KinematicsQueryOptions &options =
+                       kinematics::KinematicsQueryOptions()) const {
+    LOG_FNC();
+    return searchPositionIK(std::vector<geometry_msgs::Pose>{ik_pose},
+                            ik_seed_state, timeout, std::vector<double>(),
+                            solution, solution_callback, error_code, options);
+  }
+
+  virtual bool
+  searchPositionIK(const geometry_msgs::Pose &ik_pose,
+                   const std::vector<double> &ik_seed_state, double timeout,
+                   const std::vector<double> &consistency_limits,
+                   std::vector<double> &solution,
+                   const IKCallbackFn &solution_callback,
+                   moveit_msgs::MoveItErrorCodes &error_code,
+                   const kinematics::KinematicsQueryOptions &options =
+                       kinematics::KinematicsQueryOptions()) const {
+    LOG_FNC();
+    return searchPositionIK(std::vector<geometry_msgs::Pose>{ik_pose},
+                            ik_seed_state, timeout, consistency_limits,
+                            solution, solution_callback, error_code, options);
+  }
+
+  /*struct OptMod : kinematics::KinematicsQueryOptions
+  {
+      int test;
+  };*/
+
+  virtual bool
+  searchPositionIK(const std::vector<geometry_msgs::Pose> &ik_poses,
+                   const std::vector<double> &ik_seed_state, double timeout,
+                   const std::vector<double> &consistency_limits,
+                   std::vector<double> &solution,
+                   const IKCallbackFn &solution_callback,
+                   moveit_msgs::MoveItErrorCodes &error_code,
+                   const kinematics::KinematicsQueryOptions &options =
+                       kinematics::KinematicsQueryOptions(),
+                   const moveit::core::RobotState *context_state = NULL) const {
+    double t0 = ros::WallTime::now().toSec();
+
+    // timeout = 0.1;
+
+    // LOG("a");
+
+    if (enable_profiler)
+      Profiler::start();
+
+    auto *bio_ik_options = toBioIKKinematicsQueryOptions(&options);
+
+    LOG_FNC();
+
+    FNPROFILER();
+
+    // LOG(typeid(options).name());
+    // LOG(((OptMod*)&options)->test);
+
+    // get variable default positions / context state
+    state.resize(robot_model->getVariableCount());
+    robot_model->getVariableDefaultPositions(state);
+    if (context_state)
+      for (size_t i = 0; i < robot_model->getVariableCount(); i++)
+        state[i] = context_state->getVariablePositions()[i];
+
+    // overwrite used variables with seed state
+    solution = ik_seed_state;
+    {
+      int i = 0;
+      for (auto &joint_name : getJointNames()) {
+        auto *joint_model = robot_model->getJointModel(joint_name);
+        if (!joint_model)
+          continue;
+        for (size_t vi = 0; vi < joint_model->getVariableCount(); vi++)
+          state.at(joint_model->getFirstVariableIndex() + vi) =
+              solution.at(i++);
+      }
+    }
+
+    if (!bio_ik_options || !bio_ik_options->replace) {
+      // transform tips to baseframe
+      tipFrames.clear();
+      for (size_t i = 0; i < ik_poses.size(); i++) {
+        Eigen::Affine3d p, r;
+        tf::poseMsgToEigen(ik_poses[i], p);
+        if (context_state) {
+          r = context_state->getGlobalLinkTransform(getBaseFrame());
+        } else {
+          if (i == 0)
+            temp_state->setToDefaultValues();
+          r = temp_state->getGlobalLinkTransform(getBaseFrame());
+        }
+        tipFrames.emplace_back(r * p);
+      }
+    }
+
+    // init ik
+
+    problem.timeout = t0 + timeout;
+    problem.initial_guess = state;
+
+    // for(auto& v : state) LOG("var", &v - &state.front(), v);
+
+    // problem.tip_objectives = tipFrames;
+
+    /*for(size_t i = 0; i < problem.goals.size(); i++)
+    {
+        problem.goals[i].frame = tipFrames[i];
+    }*/
+
+    // LOG("---");
+
+    /*{
+        BLOCKPROFILER("ik goals");
+        std::vector<std::unique_ptr<Goal>> goals;
+        for(size_t i = 0; i < tip_frames_.size(); i++)
+        {
+            //if(rand() % 2) break;
+            PoseGoal* goal = new PoseGoal();
+            goal->link_name = tip_frames_[i];
+            goal->position = tipFrames[i].pos;
+            goal->orientation = tipFrames[i].rot;
+            goals.emplace_back(goal);
+            //if(rand() % 20) break;
+        }
+        //std::random_shuffle(goals.begin(), goals.end());
+        //LOG_VAR(goals.size());
+        setRequestGoals(problem, goals, ikparams);
+    }*/
+
+    {
+
+      if (!bio_ik_options || !bio_ik_options->replace) {
+        for (size_t i = 0; i < tip_frames_.size(); i++) {
+          auto *goal = (PoseGoal *)default_goals[i].get();
+          goal->setPosition(tipFrames[i].pos);
+          goal->setOrientation(tipFrames[i].rot);
+        }
+      }
+
+      all_goals.clear();
+
+      if (!bio_ik_options || !bio_ik_options->replace)
+        for (auto &goal : default_goals)
+          all_goals.push_back(goal.get());
+
+      if (bio_ik_options)
+        for (auto &goal : bio_ik_options->goals)
+          all_goals.push_back(goal.get());
+
+      {
+        BLOCKPROFILER("problem init");
+        problem.initialize(ikparams.robot_model, ikparams.joint_model_group,
+                           ikparams, all_goals, bio_ik_options);
+        // problem.setGoals(default_goals, ikparams);
+      }
+    }
+
+    {
+      BLOCKPROFILER("ik init");
+      ik->initialize(problem);
+    }
+
+    // run ik solver
+    {
+      BLOCKPROFILER("ik_solve");
+      ik->solve();
+    }
+
+    // get solution
+    state = ik->getSolution();
+
+    // wrap angles
+    for (auto ivar : problem.active_variables) {
+      auto v = state[ivar];
+      if (robot_info.isRevolute(ivar) &&
+          robot_model->getMimicJointModels().empty()) {
+        auto r = problem.initial_guess[ivar];
+        auto lo = robot_info.getMin(ivar);
+        auto hi = robot_info.getMax(ivar);
+
+        // move close to initial guess
+        if (r < v - M_PI || r > v + M_PI) {
+          v -= r;
+          v /= (2 * M_PI);
+          v += 0.5;
+          v -= std::floor(v);
+          v -= 0.5;
+          v *= (2 * M_PI);
+          v += r;
+        }
+
+        // wrap at joint limits
+        if (v > hi)
+          v -= std::ceil(std::max(0.0, v - hi) / (2 * M_PI)) * (2 * M_PI);
+        if (v < lo)
+          v += std::ceil(std::max(0.0, lo - v) / (2 * M_PI)) * (2 * M_PI);
+
+        // clamp at edges
+        if (v < lo)
+          v = lo;
+        if (v > hi)
+          v = hi;
+      }
+      state[ivar] = v;
+    }
+
+    // wrap angles
+    robot_model->enforcePositionBounds(state.data());
+
+    // map result to jointgroup variables
+    {
+      solution.clear();
+      for (auto &joint_name : getJointNames()) {
+        auto *joint_model = robot_model->getJointModel(joint_name);
+        if (!joint_model)
+          continue;
+        for (size_t vi = 0; vi < joint_model->getVariableCount(); vi++)
+          solution.push_back(
+              state.at(joint_model->getFirstVariableIndex() + vi));
+      }
+    }
+
+    // set solution fitness
+    if (bio_ik_options) {
+      bio_ik_options->solution_fitness = ik->getSolutionFitness();
+    }
+
+    // return an error if an accurate solution was requested, but no accurate
+    // solution was found
+    if (!ik->getSuccess() && !options.return_approximate_solution) {
+      error_code.val = error_code.NO_IK_SOLUTION;
+      return false;
+    }
+
+    // callback?
+    if (!solution_callback.empty()) {
+      // run callback
+      solution_callback(ik_poses.front(), solution, error_code);
+
+      // return success if callback has accepted the solution
+      return error_code.val == error_code.SUCCESS;
+    } else {
+      // return success
+      error_code.val = error_code.SUCCESS;
+      return true;
+    }
+  }
+
+  virtual bool supportsGroup(const moveit::core::JointModelGroup *jmg,
+                             std::string *error_text_out = 0) const {
+    LOG_FNC();
+    // LOG_VAR(jmg->getName());
+    return true;
+  }
+};
+} // namespace bio_ik_kinematics_plugin
+
+// register plugin
+
+#undef LOG
+#undef ERROR
+PLUGINLIB_EXPORT_CLASS(bio_ik_kinematics_plugin::BioIKKinematicsPlugin,
+                       kinematics::KinematicsBase);

src/bio_ik-kinetic-support/src/problem.cpp

@@ -0,0 +1,340 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016-2017, Philipp Sebastian Ruppel
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include "ik_base.h"
+
+#include <geometric_shapes/bodies.h>
+#include <geometric_shapes/shapes.h>
+
+#include <unordered_set>
+
+#include <mutex>
+
+#include <bio_ik/goal_types.h>
+
+namespace bio_ik
+{
+
+enum class Problem::GoalType
+{
+    Unknown,
+    Position,
+    Orientation,
+    Pose,
+};
+
+size_t Problem::addTipLink(const moveit::core::LinkModel* link_model)
+{
+    if(link_tip_indices[link_model->getLinkIndex()] < 0)
+    {
+        link_tip_indices[link_model->getLinkIndex()] = tip_link_indices.size();
+        tip_link_indices.push_back(link_model->getLinkIndex());
+    }
+    return link_tip_indices[link_model->getLinkIndex()];
+}
+
+Problem::Problem()
+    : ros_params_initrd(false)
+{
+}
+
+void Problem::initialize(moveit::core::RobotModelConstPtr robot_model, const moveit::core::JointModelGroup* joint_model_group, const IKParams& params, const std::vector<const Goal*>& goals2, const BioIKKinematicsQueryOptions* options)
+{
+    if(robot_model != this->robot_model)
+    {
+        modelInfo = RobotInfo(robot_model);
+        collision_links.clear();
+        collision_links.resize(robot_model->getLinkModelCount());
+    }
+
+    this->robot_model = robot_model;
+    this->joint_model_group = joint_model_group;
+    this->params = params;
+
+    if(!ros_params_initrd)
+    {
+        ros_params_initrd = true;
+        dpos = params.dpos;
+        drot = params.drot;
+        dtwist = params.dtwist;
+        if(dpos < 0.0 || dpos >= FLT_MAX || !std::isfinite(dpos)) dpos = DBL_MAX;
+        if(drot < 0.0 || drot >= FLT_MAX || !std::isfinite(drot)) drot = DBL_MAX;
+        if(dtwist < 0.0 || dtwist >= FLT_MAX || !std::isfinite(dtwist)) dtwist = DBL_MAX;
+    }
+
+    link_tip_indices.clear();
+    link_tip_indices.resize(robot_model->getLinkModelCount(), -1);
+    tip_link_indices.clear();
+
+    active_variables.clear();
+    auto addActiveVariable = [this, robot_model, joint_model_group, options](const std::string& name) -> ssize_t {
+        if(options)
+        {
+            auto& joint_name = robot_model->getJointOfVariable(name)->getName();
+            for(auto& fixed_joint_name : options->fixed_joints)
+            {
+                if(fixed_joint_name == joint_name)
+                {
+                    return (ssize_t)-1 - (ssize_t)robot_model->getVariableIndex(name);
+                }
+            }
+        }
+        for(size_t i = 0; i < active_variables.size(); i++)
+            if(name == robot_model->getVariableNames()[active_variables[i]]) return i;
+        for(auto& n : joint_model_group->getVariableNames())
+        {
+            if(n == name)
+            {
+                active_variables.push_back(robot_model->getVariableIndex(name));
+                return active_variables.size() - 1;
+            }
+        }
+        ERROR("joint variable not found", name);
+    };
+
+    goals.clear();
+    secondary_goals.clear();
+    for(auto& goal : goals2)
+    {
+        GoalInfo goal_info;
+
+        goal_info.goal = goal;
+
+        goal->describe(goal_info.goal_context);
+
+        for(auto& link_name : goal_info.goal_context.goal_link_names_)
+        {
+            auto* link_model = robot_model->getLinkModel(link_name);
+            if(!link_model) ERROR("link not found", link_name);
+            goal_info.goal_context.goal_link_indices_.push_back(addTipLink(link_model));
+        }
+
+        for(auto& variable_name : goal_info.goal_context.goal_variable_names_)
+        {
+            goal_info.goal_context.goal_variable_indices_.push_back(addActiveVariable(variable_name));
+        }
+
+        goal_info.weight = goal_info.goal_context.goal_weight_;
+        goal_info.weight_sq = goal_info.weight * goal_info.weight;
+
+        goal_info.goal_type = GoalType::Unknown;
+
+        goal_info.frame = Frame::identity();
+        goal_info.tip_index = 0;
+        if(goal_info.goal_context.goal_link_indices_.size()) goal_info.tip_index = goal_info.goal_context.goal_link_indices_[0];
+
+        if(auto* g = dynamic_cast<const PositionGoal*>(goal_info.goal))
+        {
+            goal_info.goal_type = GoalType::Position;
+            goal_info.frame.pos = g->getPosition();
+        }
+
+        if(auto* g = dynamic_cast<const OrientationGoal*>(goal_info.goal))
+        {
+            goal_info.goal_type = GoalType::Orientation;
+            goal_info.frame.rot = g->getOrientation();
+        }
+
+        if(auto* g = dynamic_cast<const PoseGoal*>(goal_info.goal))
+        {
+            goal_info.goal_type = GoalType::Pose;
+            goal_info.frame.pos = g->getPosition();
+            goal_info.frame.rot = g->getOrientation();
+        }
+
+        goal_info.goal_context.joint_model_group_ = joint_model_group;
+        goal_info.goal_context.initial_guess_ = initial_guess;
+
+        if(goal_info.goal_context.goal_secondary_)
+            secondary_goals.push_back(goal_info);
+        else
+            goals.push_back(goal_info);
+
+        // if(goal_info.variable_indices.size() > temp_variables.size()) temp_variables.resize(goal_info.variable_indices.size());
+
+        // if(goal_info.link_indices.size() > temp_frames.size()) temp_frames.resize(goal_info.link_indices.size());
+    }
+
+    // update active variables from active subtree
+    joint_usage.resize(robot_model->getJointModelCount());
+    for(auto& u : joint_usage)
+        u = 0;
+    for(auto tip_index : tip_link_indices)
+        for(auto* link_model = robot_model->getLinkModels()[tip_index]; link_model; link_model = link_model->getParentLinkModel())
+            joint_usage[link_model->getParentJointModel()->getJointIndex()] = 1;
+    if(options)
+        for(auto& fixed_joint_name : options->fixed_joints)
+            joint_usage[robot_model->getJointModel(fixed_joint_name)->getJointIndex()] = 0;
+    for(auto* joint_model : joint_model_group->getActiveJointModels())
+        if(joint_usage[joint_model->getJointIndex()] && !joint_model->getMimic())
+            for(auto& n : joint_model->getVariableNames())
+                addActiveVariable(n);
+
+    // init weights for minimal displacement goals
+    {
+        minimal_displacement_factors.resize(active_variables.size());
+        double s = 0;
+        for(auto ivar : active_variables)
+            s += modelInfo.getMaxVelocityRcp(ivar);
+        if(s > 0)
+        {
+            for(size_t i = 0; i < active_variables.size(); i++)
+            {
+                auto ivar = active_variables[i];
+                minimal_displacement_factors[i] = modelInfo.getMaxVelocityRcp(ivar) / s;
+            }
+        }
+        else
+        {
+            for(size_t i = 0; i < active_variables.size(); i++)
+                minimal_displacement_factors[i] = 1.0 / active_variables.size();
+        }
+    }
+
+    initialize2();
+}
+
+void Problem::initialize2()
+{
+    for(auto* gg : {&goals, &secondary_goals})
+    {
+        for(auto& g : *gg)
+        {
+            g.goal_context.problem_active_variables_ = active_variables;
+            g.goal_context.problem_tip_link_indices_ = tip_link_indices;
+            g.goal_context.velocity_weights_ = minimal_displacement_factors;
+            g.goal_context.robot_info_ = &modelInfo;
+        }
+    }
+}
+
+double Problem::computeGoalFitness(GoalInfo& goal_info, const Frame* tip_frames, const double* active_variable_positions)
+{
+    goal_info.goal_context.tip_link_frames_ = tip_frames;
+    goal_info.goal_context.active_variable_positions_ = active_variable_positions;
+    return goal_info.goal->evaluate(goal_info.goal_context) * goal_info.weight_sq;
+}
+
+double Problem::computeGoalFitness(std::vector<GoalInfo>& goals, const Frame* tip_frames, const double* active_variable_positions)
+{
+    double sum = 0.0;
+    for(auto& goal : goals)
+        sum += computeGoalFitness(goal, tip_frames, active_variable_positions);
+    return sum;
+}
+
+bool Problem::checkSolutionActiveVariables(const std::vector<Frame>& tip_frames, const double* active_variable_positions)
+{
+    for(auto& goal : goals)
+    {
+        const auto& fa = goal.frame;
+        const auto& fb = tip_frames[goal.tip_index];
+
+        switch(goal.goal_type)
+        {
+
+        case GoalType::Position:
+        {
+            if(dpos != DBL_MAX)
+            {
+                double p_dist = (fb.pos - fa.pos).length();
+                if(!(p_dist <= dpos)) return false;
+            }
+            if(dtwist != DBL_MAX)
+            {
+                KDL::Frame fk_kdl, ik_kdl;
+                frameToKDL(fa, fk_kdl);
+                frameToKDL(fb, ik_kdl);
+                KDL::Twist kdl_diff(fk_kdl.M.Inverse() * KDL::diff(fk_kdl.p, ik_kdl.p), fk_kdl.M.Inverse() * KDL::diff(fk_kdl.M, ik_kdl.M));
+                if(!KDL::Equal(kdl_diff.vel, KDL::Twist::Zero().vel, dtwist)) return false;
+            }
+            continue;
+        }
+
+        case GoalType::Orientation:
+        {
+            if(drot != DBL_MAX)
+            {
+                double r_dist = fb.rot.angleShortestPath(fa.rot);
+                r_dist = r_dist * 180 / M_PI;
+                if(!(r_dist <= drot)) return false;
+            }
+            if(dtwist != DBL_MAX)
+            {
+                KDL::Frame fk_kdl, ik_kdl;
+                frameToKDL(fa, fk_kdl);
+                frameToKDL(fb, ik_kdl);
+                KDL::Twist kdl_diff(fk_kdl.M.Inverse() * KDL::diff(fk_kdl.p, ik_kdl.p), fk_kdl.M.Inverse() * KDL::diff(fk_kdl.M, ik_kdl.M));
+                if(!KDL::Equal(kdl_diff.rot, KDL::Twist::Zero().rot, dtwist)) return false;
+            }
+            continue;
+        }
+
+        case GoalType::Pose:
+        {
+            if(dpos != DBL_MAX || drot != DBL_MAX)
+            {
+                double p_dist = (fb.pos - fa.pos).length();
+                double r_dist = fb.rot.angleShortestPath(fa.rot);
+                r_dist = r_dist * 180 / M_PI;
+                if(!(p_dist <= dpos)) return false;
+                if(!(r_dist <= drot)) return false;
+            }
+            if(dtwist != DBL_MAX)
+            {
+                KDL::Frame fk_kdl, ik_kdl;
+                frameToKDL(fa, fk_kdl);
+                frameToKDL(fb, ik_kdl);
+                KDL::Twist kdl_diff(fk_kdl.M.Inverse() * KDL::diff(fk_kdl.p, ik_kdl.p), fk_kdl.M.Inverse() * KDL::diff(fk_kdl.M, ik_kdl.M));
+                if(!KDL::Equal(kdl_diff, KDL::Twist::Zero(), dtwist)) return false;
+            }
+            continue;
+        }
+
+        default:
+        {
+            double dmax = DBL_MAX;
+            dmax = std::fmin(dmax, dpos);
+            dmax = std::fmin(dmax, dtwist);
+            double d = computeGoalFitness(goal, tip_frames.data(), active_variable_positions);
+            if(!(d < dmax * dmax)) return false;
+        }
+        }
+    }
+
+    // LOG("checkSolutionActiveVariables true");
+
+    return true;
+}
+}

src/bio_ik-kinetic-support/src/problem.h

@@ -0,0 +1,142 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016-2017, Philipp Sebastian Ruppel
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#pragma once
+
+#include "utils.h"
+#include <vector>
+
+#include <bio_ik/robot_info.h>
+
+#include <geometric_shapes/shapes.h>
+
+#include <moveit/collision_detection/collision_robot.h>
+#include <moveit/collision_detection_fcl/collision_robot_fcl.h>
+
+#include <bio_ik/goal.h>
+
+namespace bio_ik
+{
+
+class Problem
+{
+private:
+    bool ros_params_initrd;
+    std::vector<int> joint_usage;
+    std::vector<ssize_t> link_tip_indices;
+    std::vector<double> minimal_displacement_factors;
+    std::vector<double> joint_transmission_goal_temp, joint_transmission_goal_temp2;
+    moveit::core::RobotModelConstPtr robot_model;
+    const moveit::core::JointModelGroup* joint_model_group;
+    IKParams params;
+    RobotInfo modelInfo;
+    double dpos, drot, dtwist;
+    struct CollisionShape
+    {
+        std::vector<Vector3> vertices;
+        std::vector<fcl::Vec3f> points;
+        std::vector<int> polygons;
+        std::vector<fcl::Vec3f> plane_normals;
+        std::vector<double> plane_dis;
+        collision_detection::FCLGeometryConstPtr geometry;
+        Frame frame;
+        std::vector<std::vector<size_t>> edges;
+    };
+    struct CollisionLink
+    {
+        bool initialized;
+        std::vector<std::shared_ptr<CollisionShape>> shapes;
+        CollisionLink()
+            : initialized(false)
+        {
+        }
+    };
+    std::vector<CollisionLink> collision_links;
+    size_t addTipLink(const moveit::core::LinkModel* link_model);
+
+public:
+    /*enum class GoalType;
+    struct BalanceGoalInfo
+    {
+        ssize_t tip_index;
+        double mass;
+        Vector3 center;
+    };
+    struct GoalInfo
+    {
+        const Goal* goal;
+        GoalType goal_type;
+        size_t tip_index;
+        double weight;
+        double weight_sq;
+        double rotation_scale;
+        double rotation_scale_sq;
+        Frame frame;
+        tf::Vector3 target;
+        tf::Vector3 direction;
+        tf::Vector3 axis;
+        double distance;
+        ssize_t active_variable_index;
+        double variable_position;
+        std::vector<ssize_t> variable_indices;
+        mutable size_t last_collision_vertex;
+        std::vector<BalanceGoalInfo> balance_goal_infos;
+    };*/
+    enum class GoalType;
+    // std::vector<const Frame*> temp_frames;
+    // std::vector<double> temp_variables;
+    struct GoalInfo
+    {
+        const Goal* goal;
+        double weight_sq;
+        double weight;
+        GoalType goal_type;
+        size_t tip_index;
+        Frame frame;
+        GoalContext goal_context;
+    };
+    double timeout;
+    std::vector<double> initial_guess;
+    std::vector<size_t> active_variables;
+    std::vector<size_t> tip_link_indices;
+    std::vector<GoalInfo> goals;
+    std::vector<GoalInfo> secondary_goals;
+    Problem();
+    void initialize(moveit::core::RobotModelConstPtr robot_model, const moveit::core::JointModelGroup* joint_model_group, const IKParams& params, const std::vector<const Goal*>& goals2, const BioIKKinematicsQueryOptions* options);
+    void initialize2();
+    double computeGoalFitness(GoalInfo& goal, const Frame* tip_frames, const double* active_variable_positions);
+    double computeGoalFitness(std::vector<GoalInfo>& goals, const Frame* tip_frames, const double* active_variable_positions);
+    bool checkSolutionActiveVariables(const std::vector<Frame>& tip_frames, const double* active_variable_positions);
+};
+}

src/bio_ik-kinetic-support/src/utils.h

@@ -0,0 +1,521 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016-2017, Philipp Sebastian Ruppel
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#pragma once
+
+#include <csignal>
+#include <iostream>
+
+#include <ros/ros.h>
+
+#include <atomic>
+#include <mutex>
+#include <thread>
+#include <typeindex>
+#include <unordered_map>
+#include <unordered_set>
+
+#include <malloc.h>
+#include <stdlib.h>
+
+#include <tf_conversions/tf_kdl.h>
+
+#include <XmlRpcException.h>
+
+//#include <link.h>
+
+//#include <boost/align/aligned_allocator.hpp>
+//#include <Eigen/Eigen>
+
+namespace bio_ik
+{
+
+struct IKParams
+{
+    moveit::core::RobotModelConstPtr robot_model;
+    const moveit::core::JointModelGroup* joint_model_group;
+
+    // IKParallel parameters
+    std::string solver_class_name;
+    bool enable_counter;
+    int thread_count;
+
+    //Problem parameters
+    double dpos;
+    double drot;
+    double dtwist;
+
+    // ik_evolution_1 parameters
+    bool opt_no_wipeout;
+    int population_size;
+    int elite_count;
+    bool linear_fitness;
+};
+
+#define ENABLE_LOG
+
+#define ENABLE_PROFILER
+
+// logging
+
+//#define LOG_STREAM (std::cerr << std::fixed)
+//#define LOG_STREAM (std::cerr << std::scientific)
+#define LOG_STREAM (std::cerr)
+
+template <class T> inline void vprint(std::ostream& s, const T& a) { s << a << std::endl; }
+template <class T, class... AA> inline void vprint(std::ostream& s, const T& a, const AA&... aa)
+{
+    s << a << " ";
+    vprint(s, aa...);
+};
+
+#define LOG2(...) vprint(LOG_STREAM, "ikbio ", __VA_ARGS__)
+
+#ifdef ENABLE_LOG
+#define LOG(...) LOG2(__VA_ARGS__)
+#else
+#define LOG(...)
+#endif
+
+#define LOG_VAR(v) LOG2(#v, (v));
+
+//#define LOG_FNC() LOG("fun", __func__, __LINE__)
+#define LOG_FNC()
+
+// show error and abort
+// #define ERROR(...) { LOG("ERROR", __VA_ARGS__); exit(-1); }
+// #define ERROR(a, ...) { LOG(a, __VA_ARGS__); LOG_STREAM.flush(); throw std::runtime_error(a); }
+#define ERROR(...)                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                 \
+    {                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                              \
+        LOG2(__VA_ARGS__);                                                                                                                                                                                                                                                                                                                                                                                                                                                                                         \
+        LOG_STREAM.flush();                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        \
+        std::stringstream ss;                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      \
+        vprint(ss, __VA_ARGS__);                                                                                                                                                                                                                                                                                                                                                                                                                                                                                   \
+        throw std::runtime_error(ss.str());                                                                                                                                                                                                                                                                                                                                                                                                                                                                        \
+    }
+// #define ERROR(...) { LOG_ALWAYS(__VA_ARGS__); std::raise(SIGINT); }
+
+// profiler
+
+#ifdef ENABLE_PROFILER
+
+// embeddable sampling profiler
+
+// profiled block or function
+struct ProfilerBin
+{
+    const char* volatile name; // name of scope or function, also used as indicator if it is currently being executed
+    std::atomic<int> counter; // only used by CounterScope / COUNTERPROFILER
+    ProfilerBin()
+        : name(0)
+    {
+    }
+};
+
+// allocate globally unique profiler buffer via template
+template <class force_weak_linker_symbol = void> ProfilerBin* getProfilerBuffer()
+{
+    static std::vector<ProfilerBin> buffer(10000);
+    return buffer.data();
+}
+
+// reserve profiler buffer segment for current compilation unit
+template <class force_weak_linker_symbol = void> ProfilerBin* getProfilerSegment()
+{
+    static size_t index = 0;
+    return getProfilerBuffer() + (index++) * 20;
+}
+static ProfilerBin* profiler_segment = getProfilerSegment();
+
+// identifies currently profiled thread
+// null if profiler is disabled
+struct ProfilerInfo
+{
+    void* stack_begin;
+    void* stack_end;
+};
+
+// declare globally unique profiler info via template
+template <class force_weak_linker_symbol = void> ProfilerInfo& getProfilerInfo()
+{
+    static ProfilerInfo info;
+    return info;
+}
+static ProfilerInfo& profiler_info = getProfilerInfo();
+
+// profiles a scope or function
+template <size_t ID> struct ProfilerScope
+{
+    __attribute__((always_inline)) inline ProfilerScope(const char* name)
+    {
+        if(profiler_info.stack_begin == 0) return;
+        if(this < profiler_info.stack_begin || this > profiler_info.stack_end) return;
+        profiler_segment[ID].name = name;
+    }
+    __attribute__((always_inline)) inline ~ProfilerScope()
+    {
+        if(profiler_info.stack_begin == 0) return;
+        if(this < profiler_info.stack_begin || this > profiler_info.stack_end) return;
+        profiler_segment[ID].name = 0;
+    }
+};
+#define FNPROFILER() volatile ProfilerScope<__COUNTER__> _profilerscope(__func__);
+#define BLOCKPROFILER(name) volatile ProfilerScope<__COUNTER__> _profilerscope(name);
+
+// per-thread profiling
+struct ThreadScope
+{
+    size_t id;
+    __attribute__((always_inline)) inline ThreadScope(const char* name, size_t id)
+        : id(id)
+    {
+        if(profiler_info.stack_begin == 0) return;
+        profiler_segment[id].name = name;
+    }
+    __attribute__((always_inline)) inline ~ThreadScope()
+    {
+        if(profiler_info.stack_begin == 0) return;
+        profiler_segment[id].name = 0;
+    }
+};
+#define THREADPROFILER(name, id)                                                                                                                                                                                                                                                                                                                                                                                                                                                                                   \
+    static const char* _threadscope_names[] = {name "0", name "1", name "2", name "3"};                                                                                                                                                                                                                                                                                                                                                                                                                            \
+    volatile ThreadScope _threadscope(_threadscope_names[id], __COUNTER__ + id);                                                                                                                                                                                                                                                                                                                                                                                                                                   \
+    (__COUNTER__, __COUNTER__, __COUNTER__, __COUNTER__, __COUNTER__);
+
+// profiling across multiple threads
+struct CounterScope
+{
+    size_t id;
+    __attribute__((always_inline)) inline CounterScope(const char* name, size_t id)
+        : id(id)
+    {
+        if(profiler_info.stack_begin == 0) return;
+        if((profiler_segment[id].counter++) == 0) profiler_segment[id].name = name;
+    }
+    __attribute__((always_inline)) inline ~CounterScope()
+    {
+        if(profiler_info.stack_begin == 0) return;
+        if((--profiler_segment[id].counter) == 0) profiler_segment[id].name = 0;
+    }
+};
+#define COUNTERPROFILER(name) volatile CounterScope _counterscope(name, __COUNTER__);
+
+// starts profiler and periodically writes results to log
+struct Profiler
+{
+    std::thread thread;
+    volatile int exit_flag;
+    Profiler()
+    {
+        pthread_attr_t attr;
+        pthread_getattr_np(pthread_self(), &attr);
+        void* stack_addr;
+        size_t stack_size;
+        pthread_attr_getstack(&attr, &stack_addr, &stack_size);
+        profiler_info.stack_begin = stack_addr;
+        profiler_info.stack_end = (char*)stack_addr + stack_size;
+        const size_t maxbin = 1000;
+        static std::mutex mutex;
+        static std::unordered_map<const char*, size_t> samples;
+        exit_flag = 0;
+        std::thread t([this]() {
+            auto* profiler_bins = getProfilerBuffer();
+            while(true)
+            {
+                for(int iter = 0; iter < 100; iter++)
+                {
+                    for(int iter = 0; iter < 100; iter++)
+                    {
+                        int i = rand() % maxbin;
+                        const char* p = profiler_bins[i].name;
+                        if(p) samples[p]++;
+                    }
+                    if(exit_flag) break;
+                    std::this_thread::sleep_for(std::chrono::duration<size_t, std::micro>(rand() % 1000));
+                }
+                {
+                    double thistime = ros::WallTime::now().toSec();
+                    static double lasttime = 0.0;
+                    if(thistime < lasttime + 1) continue;
+                    lasttime = thistime;
+                    static std::vector<std::pair<const char*, size_t>> data;
+                    data.clear();
+                    for(auto& p : samples)
+                        data.push_back(p);
+                    std::sort(data.begin(), data.end(), [](const std::pair<const char*, size_t>& a, const std::pair<const char*, size_t>& b) { return a.second > b.second; });
+                    LOG("");
+                    LOG("profiler");
+                    for(auto& d : data)
+                    {
+                        double v = d.second * 100.0 / data[0].second;
+                        char s[32];
+                        sprintf(s, "%6.2f%%", v);
+                        LOG("p", s, d.first);
+                    }
+                    LOG("");
+                }
+                if(exit_flag) break;
+            }
+        });
+        std::swap(thread, t);
+    }
+    ~Profiler()
+    {
+        exit_flag = true;
+        thread.join();
+    }
+    static void start() { static Profiler profiler; }
+};
+
+#else
+
+#define FNPROFILER()
+#define BLOCKPROFILER(name)
+#define THREADPROFILER(name, id)
+#define COUNTERPROFILER(name)
+
+struct Profiler
+{
+    static void start() {}
+};
+
+#endif
+
+__attribute__((always_inline)) inline double mix(double a, double b, double f) { return a * (1.0 - f) + b * f; }
+
+__attribute__((always_inline)) inline double clamp(double v, double lo, double hi)
+{
+    if(v < lo) v = lo;
+    if(v > hi) v = hi;
+    return v;
+}
+
+__attribute__((always_inline)) inline double clamp2(double v, double lo, double hi)
+{
+    if(__builtin_expect(v < lo, 0)) v = lo;
+    if(__builtin_expect(v > hi, 0)) v = hi;
+    return v;
+}
+
+__attribute__((always_inline)) inline double smoothstep(float a, float b, float v)
+{
+    v = clamp((v - a) / (b - a), 0.0, 1.0);
+    return v * v * (3.0 - 2.0 * v);
+}
+
+__attribute__((always_inline)) inline double sign(double f)
+{
+    if(f < 0.0) f = -1.0;
+    if(f > 0.0) f = +1.0;
+    return f;
+}
+
+template <class t> class linear_int_distribution
+{
+    std::uniform_int_distribution<t> base;
+    t n;
+
+public:
+    inline linear_int_distribution(t vrange)
+        : n(vrange)
+        , base(0, vrange)
+    {
+    }
+    template <class generator> inline t operator()(generator& g)
+    {
+        while(true)
+        {
+            t v = base(g) + base(g);
+            if(v < n) return n - v - 1;
+        }
+    }
+};
+
+struct XORShift64
+{
+    uint64_t v;
+
+public:
+    XORShift64()
+        : v(88172645463325252ull)
+    {
+    }
+    __attribute__((always_inline)) inline uint64_t operator()()
+    {
+        v ^= v << 13;
+        v ^= v >> 7;
+        v ^= v << 17;
+        return v;
+    }
+};
+
+// class factory
+//
+// registering a class:
+//   static Factory<Base>::Class<Derived> reg("Derived");
+//
+// instantiation:
+//   Base* obj = Factory<Base>::create("Derived");
+//
+// cloning and object:
+//   p = Factory<Base>::clone(o);
+//
+template <class BASE, class... ARGS> class Factory
+{
+    typedef BASE* (*Constructor)(ARGS...);
+    struct ClassBase
+    {
+        std::string name;
+        std::type_index type;
+        virtual BASE* create(ARGS... args) const = 0;
+        virtual BASE* clone(const BASE*) const = 0;
+        ClassBase()
+            : type(typeid(void))
+        {
+        }
+    };
+    typedef std::set<ClassBase*> MapType;
+    static MapType& classes()
+    {
+        static MapType ff;
+        return ff;
+    }
+
+public:
+    template <class DERIVED> struct Class : ClassBase
+    {
+        BASE* create(ARGS... args) const { return new DERIVED(args...); }
+        BASE* clone(const BASE* o) const { return new DERIVED(*(const DERIVED*)o); }
+        Class(const std::string& name)
+        {
+            this->name = name;
+            this->type = typeid(DERIVED);
+            classes().insert(this);
+        }
+        ~Class() { classes().erase(this); }
+    };
+    static BASE* create(const std::string& name, ARGS... args)
+    {
+        for(auto* f : classes())
+            if(f->name == name) return f->create(args...);
+        ERROR("class not found", name);
+    }
+    template <class DERIVED> static DERIVED* clone(const DERIVED* o)
+    {
+        for(auto* f : classes())
+            if(f->type == typeid(*o)) return (DERIVED*)f->clone(o);
+        ERROR("class not found", typeid(*o).name());
+    }
+};
+
+// Alloctes memory properly aligned for SIMD operations
+template <class T, size_t A> struct aligned_allocator : public std::allocator<T>
+{
+    typedef size_t size_type;
+    typedef ptrdiff_t difference_type;
+    typedef T* pointer;
+    typedef const T* const_pointer;
+    typedef T& reference;
+    typedef const T& const_reference;
+    typedef T value_type;
+    T* allocate(size_t s, const void* hint = 0)
+    {
+        void* p;
+        if(posix_memalign(&p, A, sizeof(T) * s + 64)) throw std::bad_alloc();
+        return (T*)p;
+    }
+    void deallocate(T* ptr, size_t s) { free(ptr); }
+    template <class U> struct rebind
+    {
+        typedef aligned_allocator<U, A> other;
+    };
+};
+
+// std::vector typedef with proper memory alignment for SIMD operations
+template <class T> struct aligned_vector : std::vector<T, aligned_allocator<T, 32>>
+{
+};
+
+// Helper class for reading structured data from ROS parameter server
+class XmlRpcReader
+{
+    typedef XmlRpc::XmlRpcValue var;
+    var& v;
+
+public:
+    XmlRpcReader(var& v)
+        : v(v)
+    {
+    }
+
+private:
+    XmlRpcReader at(int i) { return v[i]; }
+    void conv(bool& r) { r = (bool)v; }
+    void conv(double& r) { r = (v.getType() == var::TypeInt) ? ((double)(int)v) : ((double)v); }
+    void conv(tf::Vector3& r)
+    {
+        double x, y, z;
+        at(0).conv(x);
+        at(1).conv(y);
+        at(2).conv(z);
+        r = tf::Vector3(x, y, z);
+    }
+    void conv(tf::Quaternion& r)
+    {
+        double x, y, z, w;
+        at(0).conv(x);
+        at(1).conv(y);
+        at(2).conv(z);
+        at(3).conv(w);
+        r = tf::Quaternion(x, y, z, w).normalized();
+    }
+    void conv(std::string& r) { r = (std::string)v; }
+
+public:
+    template <class T> void param(const char* key, T& r)
+    {
+        if(!v.hasMember(key)) return;
+        try
+        {
+            XmlRpcReader(v[key]).conv(r);
+        }
+        catch(const XmlRpc::XmlRpcException& e)
+        {
+            LOG(key);
+            throw;
+        }
+    }
+};
+}

src/roboglue_ros/CMakeLists.txt

@@ -0,0 +1,228 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(roboglue_ros)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  moveit_core
+  moveit_ros_planning
+  moveit_ros_planning_interface
+  moveit_visual_tools
+)
+
+find_package (Eigen3 REQUIRED)
+find_package (Boost REQUIRED system thread chrono)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+ INCLUDE_DIRS include
+ LIBRARIES roboglue_ros
+ CATKIN_DEPENDS roscpp
+ DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+ ${catkin_INCLUDE_DIRS}
+ /usr/include/eigen3
+ include
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/roboglue_ros.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+ add_executable(
+     ${PROJECT_NAME}_relay
+     src/roboglue_relay.cpp
+     src/roboglue_utils.cpp
+     )
+ add_executable(
+     ${PROJECT_NAME}_recorder
+     src/roboglue_recorder.cpp
+     src/roboglue_utils.cpp
+     )
+ add_executable(
+     ${PROJECT_NAME}_follower
+     src/roboglue_follower.cpp
+     src/roboglue_utils.cpp
+     )
+ add_executable (
+     ${PROJECT_NAME}_test
+     src/roboglue_test.cpp
+     )
+
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(
+    ${PROJECT_NAME}_test paho-mqttpp3 paho-mqtt3a ${catkin_LIBRARIES}
+)
+target_link_libraries(
+     ${PROJECT_NAME}_relay paho-mqttpp3 paho-mqtt3a ${catkin_LIBRARIES}
+)
+target_link_libraries(
+    ${PROJECT_NAME}_recorder ${catkin_LIBRARIES}
+)
+target_link_libraries(
+    ${PROJECT_NAME}_follower ${catkin_LIBRARIES}
+)
+
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_scriptpaho-mqtt3a
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+ install(TARGETS ${PROJECT_NAME}_relay ${PROJECT_NAME}_recorder ${PROJECT_NAME}_test
+   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+ )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_roboglue_ros.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

src/roboglue_ros/config/data/.data

@@ -0,0 +1,37 @@
+N,X,Y,Z,rR,rP,rY,DTDS,O1
+0,0.078000,-0.624000,0.500000,1.570000,-2.680000,0.000000,,
+1,0.078000,-0.624000,0.500000,1.570000,-2.680000,0.000000,,
+2,0.078000,-0.624000,0.500000,1.570000,-2.680000,0.000000,,
+3,0.078000,-0.624000,0.500000,1.570000,-2.680000,0.000000,,
+4,0.078000,-0.624000,0.500000,1.570000,-2.680000,0.000000,,
+5,0.078000,-0.624000,0.500000,1.570000,-2.680000,0.000000,,
+6,0.078000,-0.624000,0.500000,1.570000,-2.680000,0.000000,,
+7,0.078000,-0.624000,0.500000,1.570000,-2.680000,0.000000,,
+8,-0.054000,-0.582000,0.500000,1.570000,-2.680000,0.000000,,
+9,0.150000,-0.534000,0.500000,1.570000,-2.680000,0.000000,,
+10,0.270000,-0.456000,0.500000,1.570000,-2.680000,0.000000,,
+11,0.294000,-0.312000,0.500000,1.570000,-2.680000,0.000000,,
+12,0.366000,-0.192000,0.500000,1.570000,-2.680000,0.000000,,
+13,0.468000,-0.120000,0.500000,1.570000,-2.680000,0.000000,,
+14,0.516000,-0.138000,0.500000,1.570000,-2.680000,0.000000,,
+15,0.564000,-0.012000,0.500000,1.570000,-2.680000,0.000000,,
+16,0.450000,0.168000,0.500000,1.570000,-2.680000,0.000000,,
+17,0.360000,0.180000,0.500000,1.570000,-2.680000,0.000000,,
+18,0.210000,0.180000,0.500000,1.570000,-2.680000,0.000000,,
+19,0.102000,0.186000,0.500000,1.570000,-2.680000,0.000000,,
+20,-0.006000,0.222000,0.500000,1.570000,-2.680000,0.000000,,
+21,-0.096000,0.240000,0.500000,1.570000,-2.680000,0.000000,,
+22,-0.144000,0.240000,0.500000,1.570000,-2.680000,0.000000,,
+23,-0.234000,0.174000,0.500000,1.570000,-2.680000,0.000000,,
+24,-0.354000,-0.078000,0.500000,1.570000,-2.680000,0.000000,,
+25,-0.366000,-0.270000,0.500000,1.570000,-2.680000,0.000000,,
+26,-0.360000,-0.324000,0.500000,1.570000,-2.680000,0.000000,,
+27,-0.354000,-0.324000,0.500000,1.570000,-2.680000,0.000000,,
+28,-0.354000,-0.324000,0.500000,1.570000,-2.680000,0.000000,,
+29,-0.354000,-0.324000,0.500000,1.570000,-2.680000,0.000000,,
+30,-0.354000,-0.324000,0.500000,1.570000,-2.680000,0.000000,,
+31,-0.354000,-0.324000,0.500000,1.570000,-2.680000,0.000000,,
+32,-0.354000,-0.324000,0.500000,1.570000,-2.680000,0.000000,,
+33,-0.354000,-0.324000,0.500000,1.570000,-2.680000,0.000000,,
+34,-0.354000,-0.324000,0.500000,1.570000,-2.680000,0.000000,,
+

src/roboglue_ros/config/data/@data_template.data

@@ -0,0 +1,2 @@
+N,X,Y,Z,rR,rP,rY,DT,DS
+PIPPO,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0

src/roboglue_ros/config/data/aaaa.data

@@ -0,0 +1,54 @@
+N,X,Y,Z,rR,rP,rY,DT,DS
+N0,-0.036000,0.612000,0.500000,1.570000,-2.680000,0.000000,0.000000,0.791101
+N1,-0.252000,-0.672000,0.500000,1.570000,-2.680000,0.000000,0.800042,1.302042
+N2,-0.156000,-0.642000,0.500000,1.570000,-2.680000,0.000000,1.099722,0.100578
+N3,-0.030000,-0.636000,0.500000,1.570000,-2.680000,0.000000,1.199143,0.126143
+N4,0.012000,-0.618000,0.500000,1.570000,-2.680000,0.000000,1.300074,0.045695
+N5,0.042000,-0.618000,0.500000,1.570000,-2.680000,0.000000,1.399470,0.030000
+N6,0.072000,-0.612000,0.500000,1.570000,-2.680000,0.000000,1.499731,0.030594
+N7,0.108000,-0.600000,0.500000,1.570000,-2.680000,0.000000,1.599984,0.037947
+N8,0.114000,-0.594000,0.500000,1.570000,-2.680000,0.000000,1.699392,0.008485
+N9,0.132000,-0.588000,0.500000,1.570000,-2.680000,0.000000,1.799057,0.018974
+N10,0.150000,-0.570000,0.500000,1.570000,-2.680000,0.000000,1.908980,0.025456
+N11,0.168000,-0.552000,0.500000,1.570000,-2.680000,0.000000,1.999165,0.025456
+N12,0.180000,-0.546000,0.500000,1.570000,-2.680000,0.000000,2.099170,0.013416
+N13,0.198000,-0.534000,0.500000,1.570000,-2.680000,0.000000,2.199051,0.021633
+N14,0.222000,-0.516000,0.500000,1.570000,-2.680000,0.000000,2.299507,0.030000
+N15,0.252000,-0.498000,0.500000,1.570000,-2.680000,0.000000,2.399254,0.034986
+N16,0.294000,-0.480000,0.500000,1.570000,-2.680000,0.000000,2.499761,0.045695
+N17,0.330000,-0.456000,0.500000,1.570000,-2.680000,0.000000,2.599161,0.043267
+N18,0.396000,-0.426000,0.500000,1.570000,-2.680000,0.000000,2.709383,0.072498
+N19,0.438000,-0.396000,0.500000,1.570000,-2.680000,0.000000,2.830400,0.051614
+N20,0.474000,-0.378000,0.500000,1.570000,-2.680000,0.000000,2.899030,0.040249
+N21,0.522000,-0.324000,0.500000,1.570000,-2.680000,0.000000,2.999150,0.072250
+N22,0.540000,-0.306000,0.500000,1.570000,-2.680000,0.000000,3.099025,0.025456
+N23,0.558000,-0.246000,0.500000,1.570000,-2.680000,0.000000,3.199214,0.062642
+N24,0.570000,-0.180000,0.500000,1.570000,-2.680000,0.000000,3.299543,0.067082
+N25,0.582000,-0.114000,0.500000,1.570000,-2.680000,0.000000,3.399313,0.067082
+N26,0.582000,-0.090000,0.500000,1.570000,-2.680000,0.000000,3.509313,0.024000
+N27,0.582000,-0.030000,0.500000,1.570000,-2.680000,0.000000,3.609132,0.060000
+N28,0.558000,0.042000,0.500000,1.570000,-2.680000,0.000000,3.699045,0.075895
+N29,0.528000,0.126000,0.500000,1.570000,-2.680000,0.000000,3.799186,0.089196
+N30,0.486000,0.222000,0.500000,1.570000,-2.680000,0.000000,3.899748,0.104785
+N31,0.468000,0.240000,0.500000,1.570000,-2.680000,0.000000,3.999323,0.025456
+N32,0.450000,0.264000,0.500000,1.570000,-2.680000,0.000000,4.099438,0.030000
+N33,0.438000,0.276000,0.500000,1.570000,-2.680000,0.000000,4.199282,0.016971
+N34,0.408000,0.324000,0.500000,1.570000,-2.680000,0.000000,8.399094,0.056604
+N35,0.360000,0.366000,0.500000,1.570000,-2.680000,0.000000,8.499176,0.063781
+N36,0.306000,0.390000,0.500000,1.570000,-2.680000,0.000000,8.599155,0.059093
+N37,0.228000,0.414000,0.500000,1.570000,-2.680000,0.000000,8.709388,0.081609
+N38,0.186000,0.426000,0.500000,1.570000,-2.680000,0.000000,8.799035,0.043681
+N39,0.096000,0.438000,0.500000,1.570000,-2.680000,0.000000,8.910336,0.090796
+N40,0.000000,0.456000,0.500000,1.570000,-2.680000,0.000000,9.009624,0.097673
+N41,-0.114000,0.462000,0.500000,1.570000,-2.680000,0.000000,9.099369,0.114158
+N42,-0.222000,0.468000,0.500000,1.570000,-2.680000,0.000000,9.199063,0.108167
+N43,-0.294000,0.474000,0.500000,1.570000,-2.680000,0.000000,9.299738,0.072250
+N44,-0.348000,0.474000,0.500000,1.570000,-2.680000,0.000000,9.399080,0.054000
+N45,-0.414000,0.468000,0.500000,1.570000,-2.680000,0.000000,9.499546,0.066272
+N46,-0.432000,0.468000,0.500000,1.570000,-2.680000,0.000000,9.599448,0.018000
+N47,-0.450000,0.456000,0.500000,1.570000,-2.680000,0.000000,9.700221,0.021633
+N48,-0.456000,0.456000,0.500000,1.570000,-2.680000,0.000000,9.799106,0.006000
+N49,-0.468000,0.450000,0.500000,1.570000,-2.680000,0.000000,9.899373,0.013416
+N50,-0.486000,0.438000,0.500000,1.570000,-2.680000,0.000000,9.999050,0.021633
+N51,-0.498000,0.426000,0.500000,1.570000,-2.680000,0.000000,10.098974,0.016971
+N52,-0.498000,0.420000,0.500000,1.570000,-2.680000,0.000000,10.199223,0.006000

src/roboglue_ros/config/data/abcd.data

@@ -0,0 +1,69 @@
+N,X,Y,Z,rR,rP,rY,DT,DS
+N0,-0.006000,0.522000,0.500000,1.570000,-2.680000,0.000000,0.000000,0.000000
+N1,-0.012000,0.588000,0.500000,1.570000,-2.680000,0.000000,5.500650,0.066272
+N2,0.000000,0.588000,0.500000,1.570000,-2.680000,0.000000,5.599904,0.012000
+N3,0.060000,0.540000,0.500000,1.570000,-2.680000,0.000000,5.700698,0.076837
+N4,0.090000,0.504000,0.500000,1.570000,-2.680000,0.000000,5.800047,0.046861
+N5,0.114000,0.462000,0.500000,1.570000,-2.680000,0.000000,5.900969,0.048374
+N6,0.132000,0.438000,0.500000,1.570000,-2.680000,0.000000,6.000156,0.030000
+N7,0.156000,0.420000,0.500000,1.570000,-2.680000,0.000000,6.100259,0.030000
+N8,0.216000,0.390000,0.500000,1.570000,-2.680000,0.000000,6.200648,0.067082
+N9,0.282000,0.360000,0.500000,1.570000,-2.680000,0.000000,6.299982,0.072498
+N10,0.330000,0.336000,0.500000,1.570000,-2.680000,0.000000,6.400199,0.053666
+N11,0.348000,0.324000,0.500000,1.570000,-2.680000,0.000000,6.500126,0.021633
+N12,0.396000,0.312000,0.500000,1.570000,-2.680000,0.000000,6.600020,0.049477
+N13,0.432000,0.306000,0.500000,1.570000,-2.680000,0.000000,6.700678,0.036497
+N14,0.456000,0.294000,0.500000,1.570000,-2.680000,0.000000,6.799983,0.026833
+N15,0.486000,0.294000,0.500000,1.570000,-2.680000,0.000000,6.900591,0.030000
+N16,0.528000,0.312000,0.500000,1.570000,-2.680000,0.000000,7.000245,0.045695
+N17,0.564000,0.348000,0.500000,1.570000,-2.680000,0.000000,7.100073,0.050912
+N18,0.576000,0.366000,0.500000,1.570000,-2.680000,0.000000,7.214301,0.021633
+N19,0.582000,0.402000,0.500000,1.570000,-2.680000,0.000000,7.300039,0.036497
+N20,0.582000,0.432000,0.500000,1.570000,-2.680000,0.000000,7.400157,0.030000
+N21,0.582000,0.462000,0.500000,1.570000,-2.680000,0.000000,7.499969,0.030000
+N22,0.570000,0.504000,0.500000,1.570000,-2.680000,0.000000,7.600211,0.043681
+N23,0.546000,0.552000,0.500000,1.570000,-2.680000,0.000000,7.700129,0.053666
+N24,0.504000,0.582000,0.500000,1.570000,-2.680000,0.000000,7.800468,0.051614
+N25,0.468000,0.612000,0.500000,1.570000,-2.680000,0.000000,7.900662,0.046861
+N26,0.432000,0.642000,0.500000,1.570000,-2.680000,0.000000,7.999934,0.046861
+N27,0.396000,0.672000,0.500000,1.570000,-2.680000,0.000000,8.100259,0.046861
+N28,0.354000,0.696000,0.500000,1.570000,-2.680000,0.000000,8.200210,0.048374
+N29,0.300000,0.726000,0.500000,1.570000,-2.680000,0.000000,8.300314,0.061774
+N30,0.228000,0.750000,0.500000,1.570000,-2.680000,0.000000,8.400122,0.075895
+N31,0.174000,0.768000,0.500000,1.570000,-2.680000,0.000000,8.499868,0.056921
+N32,0.120000,0.768000,0.500000,1.570000,-2.680000,0.000000,8.599864,0.054000
+N33,0.042000,0.720000,0.500000,1.570000,-2.680000,0.000000,8.700301,0.091586
+N34,-0.018000,0.660000,0.500000,1.570000,-2.680000,0.000000,8.800045,0.084853
+N35,-0.048000,0.618000,0.500000,1.570000,-2.680000,0.000000,8.899963,0.051614
+N36,-0.078000,0.564000,0.500000,1.570000,-2.680000,0.000000,9.000003,0.061774
+N37,-0.096000,0.528000,0.500000,1.570000,-2.680000,0.000000,9.099933,0.040249
+N38,-0.114000,0.510000,0.500000,1.570000,-2.680000,0.000000,9.199871,0.025456
+N39,-0.138000,0.480000,0.500000,1.570000,-2.680000,0.000000,9.299853,0.038419
+N40,-0.168000,0.462000,0.500000,1.570000,-2.680000,0.000000,9.400127,0.034986
+N41,-0.204000,0.426000,0.500000,1.570000,-2.680000,0.000000,9.500916,0.050912
+N42,-0.246000,0.390000,0.500000,1.570000,-2.680000,0.000000,9.599930,0.055317
+N43,-0.300000,0.360000,0.500000,1.570000,-2.680000,0.000000,9.699971,0.061774
+N44,-0.402000,0.330000,0.500000,1.570000,-2.680000,0.000000,9.800119,0.106320
+N45,-0.444000,0.324000,0.500000,1.570000,-2.680000,0.000000,9.899924,0.042426
+N46,-0.474000,0.324000,0.500000,1.570000,-2.680000,0.000000,10.000206,0.030000
+N47,-0.492000,0.354000,0.500000,1.570000,-2.680000,0.000000,10.099983,0.034986
+N48,-0.534000,0.396000,0.500000,1.570000,-2.680000,0.000000,10.200459,0.059397
+N49,-0.564000,0.450000,0.500000,1.570000,-2.680000,0.000000,10.300446,0.061774
+N50,-0.570000,0.510000,0.500000,1.570000,-2.680000,0.000000,10.400013,0.060299
+N51,-0.570000,0.576000,0.500000,1.570000,-2.680000,0.000000,10.499888,0.066000
+N52,-0.558000,0.630000,0.500000,1.570000,-2.680000,0.000000,10.600098,0.055317
+N53,-0.522000,0.702000,0.500000,1.570000,-2.680000,0.000000,10.700697,0.080498
+N54,-0.486000,0.738000,0.500000,1.570000,-2.680000,0.000000,10.799913,0.050912
+N55,-0.438000,0.774000,0.500000,1.570000,-2.680000,0.000000,10.900304,0.060000
+N56,-0.402000,0.780000,0.500000,1.570000,-2.680000,0.000000,11.000116,0.036497
+N57,-0.354000,0.786000,0.500000,1.570000,-2.680000,0.000000,11.100076,0.048374
+N58,-0.300000,0.786000,0.500000,1.570000,-2.680000,0.000000,11.199880,0.054000
+N59,-0.228000,0.780000,0.500000,1.570000,-2.680000,0.000000,11.299996,0.072250
+N60,-0.156000,0.744000,0.500000,1.570000,-2.680000,0.000000,11.399813,0.080498
+N61,-0.102000,0.708000,0.500000,1.570000,-2.680000,0.000000,11.500149,0.064900
+N62,-0.066000,0.684000,0.500000,1.570000,-2.680000,0.000000,11.599973,0.043267
+N63,-0.036000,0.660000,0.500000,1.570000,-2.680000,0.000000,11.700402,0.038419
+N64,-0.030000,0.636000,0.500000,1.570000,-2.680000,0.000000,11.799870,0.024739
+N65,-0.018000,0.600000,0.500000,1.570000,-2.680000,0.000000,11.900442,0.037947
+N66,-0.012000,0.582000,0.500000,1.570000,-2.680000,0.000000,12.000309,0.018974
+N67,-0.006000,0.564000,0.500000,1.570000,-2.680000,0.000000,12.100135,0.018974

src/roboglue_ros/config/data/test01.data

@@ -0,0 +1,214 @@
+N,X,Y,Z,rR,rP,rY,DT,DS
+N0,-0.006000,0.600000,0.500000,1.570000,-2.680000,0.000000,0.000000,0.000000
+N1,0.012000,0.600000,0.500000,1.570000,-2.680000,0.000000,29.500038,0.018000
+N2,0.030000,0.594000,0.500000,1.570000,-2.680000,0.000000,29.600434,0.018974
+N3,0.048000,0.570000,0.500000,1.570000,-2.680000,0.000000,29.700142,0.030000
+N4,0.060000,0.558000,0.500000,1.570000,-2.680000,0.000000,29.800585,0.016971
+N5,0.078000,0.540000,0.500000,1.570000,-2.680000,0.000000,29.910471,0.025456
+N6,0.090000,0.528000,0.500000,1.570000,-2.680000,0.000000,30.000759,0.016971
+N7,0.090000,0.510000,0.500000,1.570000,-2.680000,0.000000,30.100790,0.018000
+N8,0.096000,0.486000,0.500000,1.570000,-2.680000,0.000000,30.200050,0.024739
+N9,0.096000,0.474000,0.500000,1.570000,-2.680000,0.000000,30.300000,0.012000
+N10,0.096000,0.444000,0.500000,1.570000,-2.680000,0.000000,30.400101,0.030000
+N11,0.108000,0.426000,0.500000,1.570000,-2.680000,0.000000,30.500265,0.021633
+N12,0.162000,0.414000,0.500000,1.570000,-2.680000,0.000000,30.600203,0.055317
+N13,0.264000,0.414000,0.500000,1.570000,-2.680000,0.000000,30.700089,0.102000
+N14,0.294000,0.420000,0.500000,1.570000,-2.680000,0.000000,30.810545,0.030594
+N15,0.318000,0.420000,0.500000,1.570000,-2.680000,0.000000,30.900354,0.024000
+N16,0.354000,0.420000,0.500000,1.570000,-2.680000,0.000000,31.000063,0.036000
+N17,0.408000,0.384000,0.500000,1.570000,-2.680000,0.000000,31.100828,0.064900
+N18,0.444000,0.348000,0.500000,1.570000,-2.680000,0.000000,31.200012,0.050912
+N19,0.474000,0.330000,0.500000,1.570000,-2.680000,0.000000,31.300212,0.034986
+N20,0.492000,0.306000,0.500000,1.570000,-2.680000,0.000000,31.409845,0.030000
+N21,0.510000,0.282000,0.500000,1.570000,-2.680000,0.000000,31.509883,0.030000
+N22,0.522000,0.246000,0.500000,1.570000,-2.680000,0.000000,31.600269,0.037947
+N23,0.534000,0.222000,0.500000,1.570000,-2.680000,0.000000,31.709772,0.026833
+N24,0.540000,0.204000,0.500000,1.570000,-2.680000,0.000000,31.799866,0.018974
+N25,0.552000,0.174000,0.500000,1.570000,-2.680000,0.000000,31.900789,0.032311
+N26,0.570000,0.150000,0.500000,1.570000,-2.680000,0.000000,32.000246,0.030000
+N27,0.594000,0.120000,0.500000,1.570000,-2.680000,0.000000,32.099988,0.038419
+N28,0.612000,0.102000,0.500000,1.570000,-2.680000,0.000000,32.199881,0.025456
+N29,0.636000,0.084000,0.500000,1.570000,-2.680000,0.000000,32.299852,0.030000
+N30,0.654000,0.066000,0.500000,1.570000,-2.680000,0.000000,32.400281,0.025456
+N31,0.672000,0.054000,0.500000,1.570000,-2.680000,0.000000,32.500291,0.021633
+N32,0.690000,0.054000,0.500000,1.570000,-2.680000,0.000000,32.600415,0.018000
+N33,0.732000,0.066000,0.500000,1.570000,-2.680000,0.000000,32.700031,0.043681
+N34,0.756000,0.078000,0.500000,1.570000,-2.680000,0.000000,32.800591,0.026833
+N35,0.798000,0.132000,0.500000,1.570000,-2.680000,0.000000,32.900711,0.068411
+N36,0.804000,0.162000,0.500000,1.570000,-2.680000,0.000000,33.000110,0.030594
+N37,0.810000,0.198000,0.500000,1.570000,-2.680000,0.000000,33.100804,0.036497
+N38,0.810000,0.216000,0.500000,1.570000,-2.680000,0.000000,33.200432,0.018000
+N39,0.816000,0.264000,0.500000,1.570000,-2.680000,0.000000,33.300402,0.048374
+N40,0.816000,0.300000,0.500000,1.570000,-2.680000,0.000000,33.400432,0.036000
+N41,0.798000,0.336000,0.500000,1.570000,-2.680000,0.000000,33.510207,0.040249
+N42,0.786000,0.390000,0.500000,1.570000,-2.680000,0.000000,33.600073,0.055317
+N43,0.768000,0.432000,0.500000,1.570000,-2.680000,0.000000,33.700262,0.045695
+N44,0.756000,0.486000,0.500000,1.570000,-2.680000,0.000000,33.800769,0.055317
+N45,0.750000,0.516000,0.500000,1.570000,-2.680000,0.000000,33.900847,0.030594
+N46,0.738000,0.546000,0.500000,1.570000,-2.680000,0.000000,34.000843,0.032311
+N47,0.732000,0.576000,0.500000,1.570000,-2.680000,0.000000,34.100415,0.030594
+N48,0.720000,0.624000,0.500000,1.570000,-2.680000,0.000000,34.210086,0.049477
+N49,0.720000,0.642000,0.500000,1.570000,-2.680000,0.000000,34.300208,0.018000
+N50,0.720000,0.654000,0.500000,1.570000,-2.680000,0.000000,34.400700,0.012000
+N51,0.714000,0.672000,0.500000,1.570000,-2.680000,0.000000,34.499923,0.018974
+N52,0.690000,0.690000,0.500000,1.570000,-2.680000,0.000000,34.600932,0.030000
+N53,0.672000,0.696000,0.500000,1.570000,-2.680000,0.000000,34.700070,0.018974
+N54,0.630000,0.720000,0.500000,1.570000,-2.680000,0.000000,34.799977,0.048374
+N55,0.582000,0.756000,0.500000,1.570000,-2.680000,0.000000,34.899948,0.060000
+N56,0.558000,0.774000,0.500000,1.570000,-2.680000,0.000000,35.000630,0.030000
+N57,0.540000,0.798000,0.500000,1.570000,-2.680000,0.000000,35.100126,0.030000
+N58,0.534000,0.804000,0.500000,1.570000,-2.680000,0.000000,35.200447,0.008485
+N59,0.516000,0.816000,0.500000,1.570000,-2.680000,0.000000,35.300680,0.021633
+N60,0.492000,0.822000,0.500000,1.570000,-2.680000,0.000000,35.400353,0.024739
+N61,0.468000,0.840000,0.500000,1.570000,-2.680000,0.000000,35.499893,0.030000
+N62,0.420000,0.858000,0.500000,1.570000,-2.680000,0.000000,35.600172,0.051264
+N63,0.396000,0.876000,0.500000,1.570000,-2.680000,0.000000,35.700541,0.030000
+N64,0.366000,0.888000,0.500000,1.570000,-2.680000,0.000000,35.800307,0.032311
+N65,0.342000,0.894000,0.500000,1.570000,-2.680000,0.000000,35.900514,0.024739
+N66,0.324000,0.894000,0.500000,1.570000,-2.680000,0.000000,36.010566,0.018000
+N67,0.294000,0.906000,0.500000,1.570000,-2.680000,0.000000,36.100511,0.032311
+N68,0.276000,0.912000,0.500000,1.570000,-2.680000,0.000000,36.200579,0.018974
+N69,0.258000,0.918000,0.500000,1.570000,-2.680000,0.000000,36.300038,0.018974
+N70,0.204000,0.930000,0.500000,1.570000,-2.680000,0.000000,36.400448,0.055317
+N71,0.162000,0.942000,0.500000,1.570000,-2.680000,0.000000,36.509928,0.043681
+N72,0.108000,0.942000,0.500000,1.570000,-2.680000,0.000000,36.600423,0.054000
+N73,0.066000,0.942000,0.500000,1.570000,-2.680000,0.000000,36.701103,0.042000
+N74,0.018000,0.942000,0.500000,1.570000,-2.680000,0.000000,36.800325,0.048000
+N75,-0.018000,0.948000,0.500000,1.570000,-2.680000,0.000000,36.900378,0.036497
+N76,-0.060000,0.948000,0.500000,1.570000,-2.680000,0.000000,37.010542,0.042000
+N77,-0.090000,0.948000,0.500000,1.570000,-2.680000,0.000000,37.100417,0.030000
+N78,-0.114000,0.948000,0.500000,1.570000,-2.680000,0.000000,37.200227,0.024000
+N79,-0.138000,0.948000,0.500000,1.570000,-2.680000,0.000000,37.300158,0.024000
+N80,-0.192000,0.948000,0.500000,1.570000,-2.680000,0.000000,37.400254,0.054000
+N81,-0.216000,0.948000,0.500000,1.570000,-2.680000,0.000000,37.500125,0.024000
+N82,-0.246000,0.948000,0.500000,1.570000,-2.680000,0.000000,37.600885,0.030000
+N83,-0.270000,0.948000,0.500000,1.570000,-2.680000,0.000000,37.700693,0.024000
+N84,-0.306000,0.948000,0.500000,1.570000,-2.680000,0.000000,37.809850,0.036000
+N85,-0.330000,0.936000,0.500000,1.570000,-2.680000,0.000000,37.900094,0.026833
+N86,-0.354000,0.936000,0.500000,1.570000,-2.680000,0.000000,38.000663,0.024000
+N87,-0.384000,0.930000,0.500000,1.570000,-2.680000,0.000000,38.100106,0.030594
+N88,-0.432000,0.906000,0.500000,1.570000,-2.680000,0.000000,38.200172,0.053666
+N89,-0.468000,0.876000,0.500000,1.570000,-2.680000,0.000000,38.300053,0.046861
+N90,-0.510000,0.846000,0.500000,1.570000,-2.680000,0.000000,38.400066,0.051614
+N91,-0.540000,0.828000,0.500000,1.570000,-2.680000,0.000000,38.500037,0.034986
+N92,-0.564000,0.804000,0.500000,1.570000,-2.680000,0.000000,38.600121,0.033941
+N93,-0.594000,0.786000,0.500000,1.570000,-2.680000,0.000000,38.710325,0.034986
+N94,-0.612000,0.768000,0.500000,1.570000,-2.680000,0.000000,38.800425,0.025456
+N95,-0.624000,0.750000,0.500000,1.570000,-2.680000,0.000000,38.900004,0.021633
+N96,-0.648000,0.726000,0.500000,1.570000,-2.680000,0.000000,39.000768,0.033941
+N97,-0.666000,0.696000,0.500000,1.570000,-2.680000,0.000000,39.100198,0.034986
+N98,-0.684000,0.654000,0.500000,1.570000,-2.680000,0.000000,39.200193,0.045695
+N99,-0.702000,0.624000,0.500000,1.570000,-2.680000,0.000000,39.301336,0.034986
+N100,-0.720000,0.606000,0.500000,1.570000,-2.680000,0.000000,39.399998,0.025456
+N101,-0.732000,0.588000,0.500000,1.570000,-2.680000,0.000000,39.500372,0.021633
+N102,-0.744000,0.570000,0.500000,1.570000,-2.680000,0.000000,39.600089,0.021633
+N103,-0.756000,0.552000,0.500000,1.570000,-2.680000,0.000000,39.700107,0.021633
+N104,-0.768000,0.504000,0.500000,1.570000,-2.680000,0.000000,39.800394,0.049477
+N105,-0.774000,0.462000,0.500000,1.570000,-2.680000,0.000000,39.900552,0.042426
+N106,-0.786000,0.438000,0.500000,1.570000,-2.680000,0.000000,40.000153,0.026833
+N107,-0.792000,0.408000,0.500000,1.570000,-2.680000,0.000000,40.100760,0.030594
+N108,-0.804000,0.384000,0.500000,1.570000,-2.680000,0.000000,40.200063,0.026833
+N109,-0.804000,0.366000,0.500000,1.570000,-2.680000,0.000000,40.300031,0.018000
+N110,-0.804000,0.342000,0.500000,1.570000,-2.680000,0.000000,40.400617,0.024000
+N111,-0.810000,0.318000,0.500000,1.570000,-2.680000,0.000000,40.501153,0.024739
+N112,-0.816000,0.300000,0.500000,1.570000,-2.680000,0.000000,40.600672,0.018974
+N113,-0.822000,0.252000,0.500000,1.570000,-2.680000,0.000000,40.700189,0.048374
+N114,-0.828000,0.204000,0.500000,1.570000,-2.680000,0.000000,40.800351,0.048374
+N115,-0.828000,0.174000,0.500000,1.570000,-2.680000,0.000000,40.900198,0.030000
+N116,-0.834000,0.138000,0.500000,1.570000,-2.680000,0.000000,41.000937,0.036497
+N117,-0.834000,0.120000,0.500000,1.570000,-2.680000,0.000000,41.100576,0.018000
+N118,-0.834000,0.096000,0.500000,1.570000,-2.680000,0.000000,41.210903,0.024000
+N119,-0.834000,0.072000,0.500000,1.570000,-2.680000,0.000000,41.300702,0.024000
+N120,-0.816000,0.036000,0.500000,1.570000,-2.680000,0.000000,41.410785,0.040249
+N121,-0.798000,0.018000,0.500000,1.570000,-2.680000,0.000000,41.500492,0.025456
+N122,-0.780000,0.006000,0.500000,1.570000,-2.680000,0.000000,41.600021,0.021633
+N123,-0.744000,-0.000000,0.500000,1.570000,-2.680000,0.000000,41.699825,0.036497
+N124,-0.690000,-0.000000,0.500000,1.570000,-2.680000,0.000000,41.800797,0.054000
+N125,-0.648000,0.012000,0.500000,1.570000,-2.680000,0.000000,41.899922,0.043681
+N126,-0.618000,0.036000,0.500000,1.570000,-2.680000,0.000000,41.999896,0.038419
+N127,-0.600000,0.060000,0.500000,1.570000,-2.680000,0.000000,42.109899,0.030000
+N128,-0.588000,0.078000,0.500000,1.570000,-2.680000,0.000000,42.210194,0.021633
+N129,-0.588000,0.102000,0.500000,1.570000,-2.680000,0.000000,42.299804,0.024000
+N130,-0.582000,0.132000,0.500000,1.570000,-2.680000,0.000000,42.400135,0.030594
+N131,-0.582000,0.162000,0.500000,1.570000,-2.680000,0.000000,42.500784,0.030000
+N132,-0.570000,0.162000,0.500000,1.570000,-2.680000,0.000000,42.600353,0.012000
+N133,-0.570000,0.180000,0.500000,1.570000,-2.680000,0.000000,42.700453,0.018000
+N134,-0.570000,0.186000,0.500000,1.570000,-2.680000,0.000000,42.804005,0.006000
+N135,-0.564000,0.198000,0.500000,1.570000,-2.680000,0.000000,42.900670,0.013416
+N136,-0.552000,0.210000,0.500000,1.570000,-2.680000,0.000000,43.000285,0.016971
+N137,-0.528000,0.246000,0.500000,1.570000,-2.680000,0.000000,43.111503,0.043267
+N138,-0.516000,0.270000,0.500000,1.570000,-2.680000,0.000000,43.200080,0.026833
+N139,-0.510000,0.288000,0.500000,1.570000,-2.680000,0.000000,43.299982,0.018974
+N140,-0.510000,0.300000,0.600000,1.570000,-2.680000,0.000000,43.400470,0.100717
+N141,-0.504000,0.318000,0.600000,1.570000,-2.680000,0.000000,43.500019,0.018974
+N142,-0.498000,0.336000,0.600000,1.570000,-2.680000,0.000000,43.600207,0.018974
+N143,-0.480000,0.354000,0.600000,1.570000,-2.680000,0.000000,43.700161,0.025456
+N144,-0.468000,0.378000,0.600000,1.570000,-2.680000,0.000000,43.809919,0.026833
+N145,-0.462000,0.390000,0.700000,1.570000,-2.680000,0.000000,43.900681,0.100896
+N146,-0.450000,0.408000,0.700000,1.570000,-2.680000,0.000000,44.000171,0.021633
+N147,-0.438000,0.414000,0.700000,1.570000,-2.680000,0.000000,44.110023,0.013416
+N148,-0.432000,0.426000,0.700000,1.570000,-2.680000,0.000000,44.200123,0.013416
+N149,-0.426000,0.432000,0.700000,1.570000,-2.680000,0.000000,44.300457,0.008485
+N150,-0.414000,0.450000,0.700000,1.570000,-2.680000,0.000000,44.399968,0.021633
+N151,-0.396000,0.468000,0.700000,1.570000,-2.680000,0.000000,44.500408,0.025456
+N152,-0.378000,0.480000,0.700000,1.570000,-2.680000,0.000000,44.599840,0.021633
+N153,-0.366000,0.492000,0.700000,1.570000,-2.680000,0.000000,44.699928,0.016971
+N154,-0.354000,0.504000,0.800000,1.570000,-2.680000,0.000000,44.799942,0.101430
+N155,-0.348000,0.516000,0.800000,1.570000,-2.680000,0.000000,44.900019,0.013416
+N156,-0.330000,0.522000,0.800000,1.570000,-2.680000,0.000000,45.000393,0.018974
+N157,-0.324000,0.528000,0.800000,1.570000,-2.680000,0.000000,45.100050,0.008485
+N158,-0.312000,0.534000,0.800000,1.570000,-2.680000,0.000000,45.200051,0.013416
+N159,-0.294000,0.534000,0.800000,1.570000,-2.680000,0.000000,45.300379,0.018000
+N160,-0.264000,0.540000,0.800000,1.570000,-2.680000,0.000000,45.399963,0.030594
+N161,-0.228000,0.552000,0.800000,1.570000,-2.680000,0.000000,45.510623,0.037947
+N162,-0.210000,0.552000,0.700000,1.570000,-2.680000,0.000000,45.600200,0.101607
+N163,-0.180000,0.552000,0.700000,1.570000,-2.680000,0.000000,45.700362,0.030000
+N164,-0.132000,0.552000,0.700000,1.570000,-2.680000,0.000000,45.809916,0.048000
+N165,-0.084000,0.552000,0.700000,1.570000,-2.680000,0.000000,45.900137,0.048000
+N166,-0.060000,0.558000,0.700000,1.570000,-2.680000,0.000000,45.999973,0.024739
+N167,-0.024000,0.558000,0.700000,1.570000,-2.680000,0.000000,46.099835,0.036000
+N168,0.006000,0.558000,0.600000,1.570000,-2.680000,0.000000,46.200010,0.104403
+N169,0.036000,0.546000,0.600000,1.570000,-2.680000,0.000000,46.300096,0.032311
+N170,0.078000,0.522000,0.600000,1.570000,-2.680000,0.000000,46.410515,0.048374
+N171,0.126000,0.510000,0.600000,1.570000,-2.680000,0.000000,46.500013,0.049477
+N172,0.168000,0.498000,0.600000,1.570000,-2.680000,0.000000,46.609856,0.043681
+N173,0.204000,0.486000,0.600000,1.570000,-2.680000,0.000000,46.710060,0.037947
+N174,0.234000,0.468000,0.600000,1.570000,-2.680000,0.000000,46.810026,0.034986
+N175,0.276000,0.450000,0.500000,1.570000,-2.680000,0.000000,46.910520,0.109945
+N176,0.306000,0.432000,0.500000,1.570000,-2.680000,0.000000,47.000245,0.034986
+N177,0.336000,0.414000,0.500000,1.570000,-2.680000,0.000000,47.100002,0.034986
+N178,0.378000,0.396000,0.500000,1.570000,-2.680000,0.000000,47.210315,0.045695
+N179,0.420000,0.360000,0.500000,1.570000,-2.680000,0.000000,47.310765,0.055317
+N180,0.456000,0.324000,0.500000,1.570000,-2.680000,0.000000,47.409829,0.050912
+N181,0.480000,0.282000,0.500000,1.570000,-2.680000,0.000000,47.499883,0.048374
+N182,0.504000,0.258000,0.400000,1.570000,-2.680000,0.000000,47.609983,0.105603
+N183,0.534000,0.246000,0.400000,1.570000,-2.680000,0.000000,47.700357,0.032311
+N184,0.552000,0.246000,0.400000,1.570000,-2.680000,0.000000,47.799916,0.018000
+N185,0.576000,0.246000,0.400000,1.570000,-2.680000,0.000000,47.900094,0.024000
+N186,0.600000,0.258000,0.400000,1.570000,-2.680000,0.000000,48.000150,0.026833
+N187,0.606000,0.288000,0.400000,1.570000,-2.680000,0.000000,48.100320,0.030594
+N188,0.606000,0.330000,0.400000,1.570000,-2.680000,0.000000,48.199939,0.042000
+N189,0.606000,0.366000,0.400000,1.570000,-2.680000,0.000000,48.300106,0.036000
+N190,0.594000,0.396000,0.500000,1.570000,-2.680000,0.000000,48.409848,0.105090
+N191,0.576000,0.426000,0.500000,1.570000,-2.680000,0.000000,48.500494,0.034986
+N192,0.552000,0.468000,0.500000,1.570000,-2.680000,0.000000,48.601117,0.048374
+N193,0.534000,0.498000,0.500000,1.570000,-2.680000,0.000000,48.700411,0.034986
+N194,0.510000,0.528000,0.500000,1.570000,-2.680000,0.000000,48.799952,0.038419
+N195,0.492000,0.546000,0.500000,1.570000,-2.680000,0.000000,48.900393,0.025456
+N196,0.462000,0.576000,0.500000,1.570000,-2.680000,0.000000,49.009898,0.042426
+N197,0.432000,0.594000,0.600000,1.570000,-2.680000,0.000000,49.110086,0.105943
+N198,0.384000,0.606000,0.600000,1.570000,-2.680000,0.000000,49.200135,0.049477
+N199,0.330000,0.618000,0.600000,1.570000,-2.680000,0.000000,49.309860,0.055317
+N200,0.300000,0.618000,0.600000,1.570000,-2.680000,0.000000,49.400580,0.030000
+N201,0.264000,0.624000,0.600000,1.570000,-2.680000,0.000000,49.500009,0.036497
+N202,0.246000,0.624000,0.600000,1.570000,-2.680000,0.000000,49.599972,0.018000
+N203,0.216000,0.624000,0.700000,1.570000,-2.680000,0.000000,49.699869,0.104403
+N204,0.156000,0.612000,0.700000,1.570000,-2.680000,0.000000,49.800317,0.061188
+N205,0.102000,0.600000,0.700000,1.570000,-2.680000,0.000000,49.899842,0.055317
+N206,0.072000,0.600000,0.700000,1.570000,-2.680000,0.000000,50.000131,0.030000
+N207,0.042000,0.588000,0.700000,1.570000,-2.680000,0.000000,50.100185,0.032311
+N208,0.006000,0.588000,0.700000,1.570000,-2.680000,0.000000,50.200599,0.036000
+N209,-0.012000,0.588000,0.700000,1.570000,-2.680000,0.000000,50.300299,0.018000
+N210,-0.024000,0.582000,0.700000,1.570000,-2.680000,0.000000,50.400747,0.013416
+N211,-0.036000,0.582000,0.800000,1.570000,-2.680000,0.000000,50.600925,0.100717
+N212,-0.042000,0.582000,0.800000,1.570000,-2.680000,0.000000,50.700502,0.006000

src/roboglue_ros/config/data/test02.data

@@ -0,0 +1,43 @@
+N,X,Y,Z,rR,rP,rY,DT,DS
+N0,0.228000,0.000000,0.500000,1.570000,-2.680000,0.000000,0.000000,0.000000
+N1,0.204000,0.000000,0.500000,1.570000,-2.680000,0.000000,0.100073,0.024000
+N2,0.156000,0.000000,0.500000,1.570000,-2.680000,0.000000,0.200256,0.048000
+N3,0.144000,0.000000,0.500000,1.570000,-2.680000,0.000000,0.300186,0.012000
+N4,0.126000,0.000000,0.500000,1.570000,-2.680000,0.000000,0.399959,0.018000
+N5,0.084000,0.000000,0.500000,1.570000,-2.680000,0.000000,0.500295,0.042000
+N6,0.054000,0.000000,0.500000,1.570000,-2.680000,0.000000,0.600313,0.030000
+N7,0.036000,0.000000,0.500000,1.570000,-2.680000,0.000000,0.699965,0.018000
+N8,0.030000,0.000000,0.500000,1.570000,-2.680000,0.000000,0.800272,0.006000
+N9,0.000000,0.000000,0.500000,1.570000,-2.680000,0.000000,0.910034,0.030000
+N10,-0.036000,0.000000,0.500000,1.570000,-2.680000,0.000000,1.000029,0.036000
+N11,-0.090000,0.000000,0.500000,1.570000,-2.680000,0.000000,1.100357,0.054000
+N12,-0.126000,0.000000,0.500000,1.570000,-2.680000,0.000000,1.200191,0.036000
+N13,-0.180000,0.000000,0.500000,1.570000,-2.680000,0.000000,1.300034,0.054000
+N14,-0.210000,0.000000,0.500000,1.570000,-2.680000,0.000000,1.399941,0.030000
+N15,-0.216000,0.000000,0.500000,1.570000,-2.680000,0.000000,1.501023,0.006000
+N16,-0.204000,0.000000,0.500000,1.570000,-2.680000,0.000000,1.700007,0.012000
+N17,-0.198000,0.000000,0.500000,1.570000,-2.680000,0.000000,1.799972,0.006000
+N18,-0.144000,0.000000,0.500000,1.570000,-2.680000,0.000000,2.001025,0.054000
+N19,-0.030000,0.000000,0.500000,1.570000,-2.680000,0.000000,2.109982,0.114000
+N20,0.024000,0.000000,0.500000,1.570000,-2.680000,0.000000,2.200092,0.054000
+N21,0.078000,0.000000,0.500000,1.570000,-2.680000,0.000000,2.310167,0.054000
+N22,0.138000,0.000000,0.500000,1.570000,-2.680000,0.000000,2.410010,0.060000
+N23,0.180000,0.000000,0.500000,1.570000,-2.680000,0.000000,2.499971,0.042000
+N24,0.264000,0.000000,0.500000,1.570000,-2.680000,0.000000,2.600115,0.084000
+N25,0.342000,0.000000,0.500000,1.570000,-2.680000,0.000000,2.700064,0.078000
+N26,0.432000,0.000000,0.500000,1.570000,-2.680000,0.000000,2.800222,0.090000
+N27,0.534000,0.000000,0.500000,1.570000,-2.680000,0.000000,2.900607,0.102000
+N28,0.582000,0.000000,0.500000,1.570000,-2.680000,0.000000,3.001229,0.048000
+N29,0.534000,0.000000,0.500000,1.570000,-2.680000,0.000000,3.210495,0.048000
+N30,0.360000,0.000000,0.500000,1.570000,-2.680000,0.000000,3.310035,0.174000
+N31,0.174000,0.000000,0.500000,1.570000,-2.680000,0.000000,3.400096,0.186000
+N32,0.138000,0.000000,0.500000,1.570000,-2.680000,0.000000,3.500059,0.036000
+N33,0.102000,0.000000,0.500000,1.570000,-2.680000,0.000000,3.600201,0.036000
+N34,0.084000,0.000000,0.500000,1.570000,-2.680000,0.000000,3.700504,0.018000
+N35,-0.258000,0.000000,0.500000,1.570000,-2.680000,0.000000,4.199981,0.342000
+N36,-0.216000,0.000000,0.500000,1.570000,-2.680000,0.000000,4.300291,0.042000
+N37,0.174000,0.000000,0.500000,1.570000,-2.680000,0.000000,4.810028,0.390000
+N38,0.216000,0.000000,0.500000,1.570000,-2.680000,0.000000,4.900141,0.042000
+N39,0.258000,0.000000,0.500000,1.570000,-2.680000,0.000000,5.000439,0.042000
+N40,0.288000,0.000000,0.500000,1.570000,-2.680000,0.000000,5.100986,0.030000
+N41,0.294000,0.000000,0.500000,1.570000,-2.680000,0.000000,5.301044,0.006000

src/roboglue_ros/config/data/test03.data

@@ -0,0 +1,37 @@
+N,X,Y,Z,rR,rP,rY,DT,DS
+N0,0.294000,0.000000,0.500000,1.570000,-2.680000,0.000000,0.000000,0.000000
+N1,0.402000,0.000000,0.500000,1.570000,-2.680000,0.000000,1.401285,0.108000
+N2,0.390000,0.000000,0.500000,1.570000,-2.680000,0.000000,1.490633,0.012000
+N3,0.264000,0.000000,0.500000,1.570000,-2.680000,0.000000,1.720621,0.126000
+N4,0.198000,0.000000,0.500000,1.570000,-2.680000,0.000000,1.790100,0.066000
+N5,0.126000,0.000000,0.500000,1.570000,-2.680000,0.000000,1.890083,0.072000
+N6,0.060000,0.000000,0.500000,1.570000,-2.680000,0.000000,1.990527,0.066000
+N7,0.006000,0.000000,0.500000,1.570000,-2.680000,0.000000,2.090623,0.054000
+N8,-0.024000,0.000000,0.500000,1.570000,-2.680000,0.000000,2.190201,0.030000
+N9,-0.066000,0.000000,0.500000,1.570000,-2.680000,0.000000,2.290082,0.042000
+N10,-0.126000,0.000000,0.500000,1.570000,-2.680000,0.000000,2.400254,0.060000
+N11,-0.162000,0.000000,0.500000,1.570000,-2.680000,0.000000,2.490008,0.036000
+N12,-0.216000,0.000000,0.500000,1.570000,-2.680000,0.000000,2.590011,0.054000
+N13,-0.270000,0.000000,0.500000,1.570000,-2.680000,0.000000,2.689965,0.054000
+N14,-0.318000,0.000000,0.500000,1.570000,-2.680000,0.000000,2.790556,0.048000
+N15,-0.336000,0.000000,0.500000,1.570000,-2.680000,0.000000,2.890059,0.018000
+N16,-0.378000,0.000000,0.500000,1.570000,-2.680000,0.000000,2.989947,0.042000
+N17,-0.438000,0.000000,0.500000,1.570000,-2.680000,0.000000,3.090571,0.060000
+N18,-0.480000,0.000000,0.500000,1.570000,-2.680000,0.000000,3.190113,0.042000
+N19,-0.510000,0.000000,0.500000,1.570000,-2.680000,0.000000,3.290084,0.030000
+N20,-0.528000,0.000000,0.500000,1.570000,-2.680000,0.000000,3.389943,0.018000
+N21,-0.522000,0.000000,0.500000,1.570000,-2.680000,0.000000,3.490660,0.006000
+N22,-0.492000,0.000000,0.500000,1.570000,-2.680000,0.000000,3.590991,0.030000
+N23,-0.432000,0.000000,0.500000,1.570000,-2.680000,0.000000,3.690180,0.060000
+N24,-0.366000,0.000000,0.500000,1.570000,-2.680000,0.000000,3.790109,0.066000
+N25,-0.258000,0.000000,0.500000,1.570000,-2.680000,0.000000,3.890116,0.108000
+N26,-0.180000,0.000000,0.500000,1.570000,-2.680000,0.000000,3.990109,0.078000
+N27,-0.066000,0.000000,0.500000,1.570000,-2.680000,0.000000,4.089970,0.114000
+N28,0.042000,0.000000,0.500000,1.570000,-2.680000,0.000000,4.199993,0.108000
+N29,0.108000,0.000000,0.500000,1.570000,-2.680000,0.000000,4.291120,0.066000
+N30,0.174000,0.000000,0.500000,1.570000,-2.680000,0.000000,4.390608,0.066000
+N31,0.246000,0.000000,0.500000,1.570000,-2.680000,0.000000,4.490012,0.072000
+N32,0.294000,0.000000,0.500000,1.570000,-2.680000,0.000000,4.590249,0.048000
+N33,0.336000,0.000000,0.500000,1.570000,-2.680000,0.000000,4.690568,0.042000
+N34,0.348000,0.000000,0.500000,1.570000,-2.680000,0.000000,4.790000,0.012000
+N35,0.354000,0.000000,0.500000,1.570000,-2.680000,0.000000,4.890584,0.006000

src/roboglue_ros/config/data/test04.data

@@ -0,0 +1,117 @@
+N,X,Y,Z,rR,rP,rY,DT,DS
+N0,0.516000,0.534000,0.500000,1.570000,-2.680000,0.000000,0.000000,0.000000
+N1,-0.030000,0.528000,0.500000,1.570000,-2.680000,0.000000,1.699972,0.546033
+N2,-0.006000,0.528000,0.500000,1.570000,-2.680000,0.000000,1.799936,0.024000
+N3,0.036000,0.498000,0.500000,1.570000,-2.680000,0.000000,1.900095,0.051614
+N4,0.078000,0.468000,0.500000,1.570000,-2.680000,0.000000,2.000116,0.051614
+N5,0.120000,0.438000,0.500000,1.570000,-2.680000,0.000000,2.100071,0.051614
+N6,0.138000,0.426000,0.500000,1.570000,-2.680000,0.000000,2.210149,0.021633
+N7,0.162000,0.402000,0.500000,1.570000,-2.680000,0.000000,2.310041,0.033941
+N8,0.186000,0.390000,0.500000,1.570000,-2.680000,0.000000,2.400485,0.026833
+N9,0.228000,0.390000,0.500000,1.570000,-2.680000,0.000000,2.500225,0.042000
+N10,0.282000,0.396000,0.500000,1.570000,-2.680000,0.000000,2.600569,0.054332
+N11,0.342000,0.414000,0.500000,1.570000,-2.680000,0.000000,2.700142,0.062642
+N12,0.390000,0.444000,0.500000,1.570000,-2.680000,0.000000,2.800152,0.056604
+N13,0.408000,0.480000,0.500000,1.570000,-2.680000,0.000000,2.910234,0.040249
+N14,0.426000,0.504000,0.500000,1.570000,-2.680000,0.000000,3.000374,0.030000
+N15,0.432000,0.540000,0.500000,1.570000,-2.680000,0.000000,3.109967,0.036497
+N16,0.426000,0.588000,0.500000,1.570000,-2.680000,0.000000,3.200046,0.048374
+N17,0.396000,0.624000,0.500000,1.570000,-2.680000,0.000000,3.300258,0.046861
+N18,0.360000,0.666000,0.500000,1.570000,-2.680000,0.000000,3.401080,0.055317
+N19,0.306000,0.702000,0.500000,1.570000,-2.680000,0.000000,3.510613,0.064900
+N20,0.252000,0.714000,0.500000,1.570000,-2.680000,0.000000,3.600382,0.055317
+N21,0.060000,0.702000,0.500000,1.570000,-2.680000,0.000000,3.700211,0.192375
+N22,0.048000,0.690000,0.500000,1.570000,-2.680000,0.000000,3.800090,0.016971
+N23,0.006000,0.618000,0.500000,1.570000,-2.680000,0.000000,3.900192,0.083355
+N24,-0.048000,0.558000,0.500000,1.570000,-2.680000,0.000000,3.999904,0.080722
+N25,-0.090000,0.516000,0.500000,1.570000,-2.680000,0.000000,4.099924,0.059397
+N26,-0.150000,0.486000,0.500000,1.570000,-2.680000,0.000000,4.200352,0.067082
+N27,-0.192000,0.474000,0.500000,1.570000,-2.680000,0.000000,4.311069,0.043681
+N28,-0.252000,0.522000,0.500000,1.570000,-2.680000,0.000000,4.399919,0.076837
+N29,-0.288000,0.588000,0.500000,1.570000,-2.680000,0.000000,4.500244,0.075180
+N30,-0.312000,0.612000,0.500000,1.570000,-2.680000,0.000000,4.600301,0.033941
+N31,-0.342000,0.618000,0.500000,1.570000,-2.680000,0.000000,4.700040,0.030594
+N32,-0.384000,0.552000,0.500000,1.570000,-2.680000,0.000000,4.799930,0.078230
+N33,-0.396000,0.510000,0.500000,1.570000,-2.680000,0.000000,4.900146,0.043681
+N34,-0.420000,0.456000,0.500000,1.570000,-2.680000,0.000000,5.000464,0.059093
+N35,-0.474000,0.414000,0.500000,1.570000,-2.680000,0.000000,5.100255,0.068411
+N36,-0.480000,0.432000,0.500000,1.570000,-2.680000,0.000000,5.200699,0.018974
+N37,-0.510000,0.516000,0.500000,1.570000,-2.680000,0.000000,5.300193,0.089196
+N38,-0.540000,0.582000,0.500000,1.570000,-2.680000,0.000000,5.400096,0.072498
+N39,-0.624000,0.606000,0.500000,1.570000,-2.680000,0.000000,5.509928,0.087361
+N40,-0.684000,0.576000,0.500000,1.570000,-2.680000,0.000000,5.599922,0.067082
+N41,-0.696000,0.516000,0.500000,1.570000,-2.680000,0.000000,5.700124,0.061188
+N42,-0.696000,0.444000,0.500000,1.570000,-2.680000,0.000000,5.799904,0.072000
+N43,-0.702000,0.390000,0.500000,1.570000,-2.680000,0.000000,5.910562,0.054332
+N44,-0.732000,0.348000,0.500000,1.570000,-2.680000,0.000000,6.000061,0.051614
+N45,-0.744000,0.348000,0.500000,1.570000,-2.680000,0.000000,6.100050,0.012000
+N46,-0.780000,0.348000,0.500000,1.570000,-2.680000,0.000000,6.200066,0.036000
+N47,-0.798000,0.402000,0.500000,1.570000,-2.680000,0.000000,6.300133,0.056921
+N48,-0.780000,0.462000,0.500000,1.570000,-2.680000,0.000000,6.400315,0.062642
+N49,-0.720000,0.558000,0.500000,1.570000,-2.680000,0.000000,6.510403,0.113208
+N50,-0.690000,0.576000,0.500000,1.570000,-2.680000,0.000000,6.631582,0.034986
+N51,-0.648000,0.522000,0.500000,1.570000,-2.680000,0.000000,6.720484,0.068411
+N52,-0.618000,0.456000,0.500000,1.570000,-2.680000,0.000000,6.800401,0.072498
+N53,-0.576000,0.420000,0.500000,1.570000,-2.680000,0.000000,6.900349,0.055317
+N54,-0.552000,0.414000,0.500000,1.570000,-2.680000,0.000000,6.999950,0.024739
+N55,-0.510000,0.474000,0.500000,1.570000,-2.680000,0.000000,7.100683,0.073239
+N56,-0.492000,0.582000,0.500000,1.570000,-2.680000,0.000000,7.200196,0.109490
+N57,-0.468000,0.672000,0.500000,1.570000,-2.680000,0.000000,7.300141,0.093145
+N58,-0.438000,0.684000,0.500000,1.570000,-2.680000,0.000000,7.400230,0.032311
+N59,-0.330000,0.600000,0.500000,1.570000,-2.680000,0.000000,7.500249,0.136821
+N60,-0.318000,0.558000,0.500000,1.570000,-2.680000,0.000000,7.690120,0.043681
+N61,-0.306000,0.450000,0.500000,1.570000,-2.680000,0.000000,7.700197,0.108665
+N62,-0.270000,0.456000,0.500000,1.570000,-2.680000,0.000000,7.800371,0.036497
+N63,-0.228000,0.516000,0.500000,1.570000,-2.680000,0.000000,7.899930,0.073239
+N64,-0.192000,0.672000,0.500000,1.570000,-2.680000,0.000000,8.000629,0.160100
+N65,-0.168000,0.708000,0.500000,1.570000,-2.680000,0.000000,8.100009,0.043267
+N66,-0.114000,0.714000,0.500000,1.570000,-2.680000,0.000000,8.200327,0.054332
+N67,-0.054000,0.630000,0.500000,1.570000,-2.680000,0.000000,8.299947,0.103228
+N68,-0.036000,0.516000,0.500000,1.570000,-2.680000,0.000000,8.400531,0.115412
+N69,0.000000,0.474000,0.500000,1.570000,-2.680000,0.000000,8.500066,0.055317
+N70,0.048000,0.492000,0.500000,1.570000,-2.680000,0.000000,8.600167,0.051264
+N71,0.078000,0.540000,0.500000,1.570000,-2.680000,0.000000,8.930607,0.056604
+N72,0.342000,0.684000,0.500000,1.570000,-2.680000,0.000000,9.000188,0.300719
+N73,0.372000,0.558000,0.500000,1.570000,-2.680000,0.000000,9.100174,0.129522
+N74,0.312000,0.522000,0.500000,1.570000,-2.680000,0.000000,9.200245,0.069971
+N75,0.276000,0.462000,0.500000,1.570000,-2.680000,0.000000,9.300049,0.069971
+N76,0.306000,0.384000,0.500000,1.570000,-2.680000,0.000000,9.410188,0.083570
+N77,0.384000,0.366000,0.500000,1.570000,-2.680000,0.000000,9.500611,0.080050
+N78,0.432000,0.510000,0.500000,1.570000,-2.680000,0.000000,9.600219,0.151789
+N79,0.456000,0.528000,0.500000,1.570000,-2.680000,0.000000,9.700259,0.030000
+N80,0.528000,0.474000,0.500000,1.570000,-2.680000,0.000000,9.799969,0.090000
+N81,0.564000,0.402000,0.500000,1.570000,-2.680000,0.000000,9.900037,0.080498
+N82,0.504000,0.372000,0.500000,1.570000,-2.680000,0.000000,9.999918,0.067082
+N83,0.510000,0.252000,0.500000,1.570000,-2.680000,0.000000,10.100475,0.120150
+N84,0.570000,0.204000,0.500000,1.570000,-2.680000,0.000000,10.200536,0.076837
+N85,0.624000,0.348000,0.500000,1.570000,-2.680000,0.000000,10.300144,0.153792
+N86,0.654000,0.390000,0.500000,1.570000,-2.680000,0.000000,10.410394,0.051614
+N87,0.726000,0.330000,0.500000,1.570000,-2.680000,0.000000,10.499924,0.093723
+N88,0.762000,0.204000,0.500000,1.570000,-2.680000,0.000000,10.600156,0.131042
+N89,0.636000,0.210000,0.500000,1.570000,-2.680000,0.000000,10.700094,0.126143
+N90,0.546000,0.492000,0.500000,1.570000,-2.680000,0.000000,10.940099,0.296014
+N91,0.510000,0.582000,0.500000,1.570000,-2.680000,0.000000,11.000160,0.096933
+N92,0.408000,0.660000,0.500000,1.570000,-2.680000,0.000000,11.100200,0.128406
+N93,0.360000,0.582000,0.500000,1.570000,-2.680000,0.000000,11.200304,0.091586
+N94,0.348000,0.498000,0.500000,1.570000,-2.680000,0.000000,11.300025,0.084853
+N95,0.390000,0.540000,0.500000,1.570000,-2.680000,0.000000,11.400072,0.059397
+N96,0.366000,0.684000,0.500000,1.570000,-2.680000,0.000000,11.499870,0.145986
+N97,0.288000,0.762000,0.500000,1.570000,-2.680000,0.000000,11.600217,0.110309
+N98,0.204000,0.774000,0.500000,1.570000,-2.680000,0.000000,11.715872,0.084853
+N99,0.156000,0.684000,0.500000,1.570000,-2.680000,0.000000,11.798840,0.102000
+N100,0.162000,0.630000,0.500000,1.570000,-2.680000,0.000000,11.899556,0.054332
+N101,0.168000,0.546000,0.500000,1.570000,-2.680000,0.000000,12.019110,0.084214
+N102,0.126000,0.498000,0.500000,1.570000,-2.680000,0.000000,12.098981,0.063781
+N103,0.066000,0.486000,0.500000,1.570000,-2.680000,0.000000,12.199683,0.061188
+N104,0.048000,0.552000,0.500000,1.570000,-2.680000,0.000000,12.299024,0.068411
+N105,0.030000,0.642000,0.500000,1.570000,-2.680000,0.000000,12.398961,0.091782
+N106,-0.138000,0.756000,0.500000,1.570000,-2.680000,0.000000,12.498962,0.203027
+N107,-0.294000,0.756000,0.500000,1.570000,-2.680000,0.000000,12.598987,0.156000
+N108,-0.360000,0.678000,0.500000,1.570000,-2.680000,0.000000,12.698785,0.102176
+N109,-0.360000,0.558000,0.500000,1.570000,-2.680000,0.000000,12.798908,0.120000
+N110,-0.372000,0.522000,0.500000,1.570000,-2.680000,0.000000,12.898959,0.037947
+N111,-0.408000,0.498000,0.500000,1.570000,-2.680000,0.000000,12.998913,0.043267
+N112,-0.474000,0.534000,0.500000,1.570000,-2.680000,0.000000,13.098895,0.075180
+N113,-0.486000,0.588000,0.500000,1.570000,-2.680000,0.000000,13.199388,0.055317
+N114,-0.468000,0.618000,0.500000,1.570000,-2.680000,0.000000,13.298969,0.034986
+N115,-0.456000,0.624000,0.500000,1.570000,-2.680000,0.000000,13.398841,0.013416

src/roboglue_ros/config/data/test05.data

@@ -0,0 +1,49 @@
+N,X,Y,Z,rR,rP,rY,DT,DS
+N0,-0.456000,0.624000,0.500000,1.570000,-2.680000,0.000000,0.000000,0.000000
+N1,0.426000,0.384000,0.500000,1.570000,-2.680000,0.000000,1.860674,0.914070
+N2,0.486000,0.414000,0.500000,1.570000,-2.680000,0.000000,1.960324,0.067082
+N3,0.510000,0.456000,0.500000,1.570000,-2.680000,0.000000,2.060077,0.048374
+N4,0.534000,0.510000,0.500000,1.570000,-2.680000,0.000000,2.159825,0.059093
+N5,0.534000,0.576000,0.500000,1.570000,-2.680000,0.000000,2.260192,0.066000
+N6,0.510000,0.642000,0.500000,1.570000,-2.680000,0.000000,2.359822,0.070228
+N7,0.468000,0.714000,0.500000,1.570000,-2.680000,0.000000,2.459952,0.083355
+N8,0.408000,0.762000,0.500000,1.570000,-2.680000,0.000000,2.560109,0.076837
+N9,0.102000,0.720000,0.500000,1.570000,-2.680000,0.000000,3.110003,0.308869
+N10,0.072000,0.660000,0.500000,1.570000,-2.680000,0.000000,3.220451,0.067082
+N11,0.054000,0.606000,0.500000,1.570000,-2.680000,0.000000,3.320072,0.056921
+N12,0.030000,0.546000,0.500000,1.570000,-2.680000,0.000000,3.430375,0.064622
+N13,0.012000,0.492000,0.500000,1.570000,-2.680000,0.000000,3.530369,0.056921
+N14,-0.012000,0.426000,0.500000,1.570000,-2.680000,0.000000,3.640076,0.070228
+N15,-0.066000,0.360000,0.500000,1.570000,-2.680000,0.000000,3.740103,0.085276
+N16,-0.108000,0.336000,0.500000,1.570000,-2.680000,0.000000,3.849812,0.048374
+N17,-0.156000,0.306000,0.500000,1.570000,-2.680000,0.000000,3.951439,0.056604
+N18,-0.216000,0.306000,0.500000,1.570000,-2.680000,0.000000,4.050171,0.060000
+N19,-0.300000,0.306000,0.500000,1.570000,-2.680000,0.000000,4.159946,0.084000
+N20,-0.396000,0.330000,0.500000,1.570000,-2.680000,0.000000,4.260629,0.098955
+N21,-0.516000,0.396000,0.500000,1.570000,-2.680000,0.000000,4.360201,0.136953
+N22,-0.564000,0.474000,0.500000,1.570000,-2.680000,0.000000,4.459862,0.091586
+N23,-0.576000,0.522000,0.500000,1.570000,-2.680000,0.000000,4.549892,0.049477
+N24,-0.576000,0.582000,0.500000,1.570000,-2.680000,0.000000,4.660502,0.060000
+N25,-0.552000,0.648000,0.500000,1.570000,-2.680000,0.000000,4.760301,0.070228
+N26,-0.504000,0.708000,0.500000,1.570000,-2.680000,0.000000,4.850300,0.076837
+N27,-0.414000,0.744000,0.500000,1.570000,-2.680000,0.000000,4.960418,0.096933
+N28,-0.360000,0.744000,0.500000,1.570000,-2.680000,0.000000,5.060370,0.054000
+N29,-0.282000,0.720000,0.500000,1.570000,-2.680000,0.000000,5.159877,0.081609
+N30,-0.210000,0.672000,0.500000,1.570000,-2.680000,0.000000,5.260510,0.086533
+N31,-0.114000,0.612000,0.500000,1.570000,-2.680000,0.000000,5.359936,0.113208
+N32,-0.024000,0.552000,0.500000,1.570000,-2.680000,0.000000,5.459927,0.108167
+N33,-0.006000,0.528000,0.500000,1.570000,-2.680000,0.000000,5.991008,0.030000
+N34,0.444000,0.234000,0.500000,1.570000,-2.680000,0.000000,6.059999,0.537528
+N35,0.528000,0.252000,0.500000,1.570000,-2.680000,0.000000,6.160406,0.085907
+N36,0.624000,0.330000,0.500000,1.570000,-2.680000,0.000000,6.260150,0.123693
+N37,0.684000,0.570000,0.500000,1.570000,-2.680000,0.000000,6.470083,0.247386
+N38,0.636000,0.684000,0.500000,1.570000,-2.680000,0.000000,6.560120,0.123693
+N39,0.558000,0.810000,0.500000,1.570000,-2.680000,0.000000,6.660172,0.148189
+N40,0.468000,0.882000,0.500000,1.570000,-2.680000,0.000000,6.760186,0.115256
+N41,0.372000,0.942000,0.500000,1.570000,-2.680000,0.000000,6.860101,0.113208
+N42,0.312000,0.954000,0.500000,1.570000,-2.680000,0.000000,6.959798,0.061188
+N43,0.228000,0.954000,0.500000,1.570000,-2.680000,0.000000,7.059984,0.084000
+N44,0.114000,0.936000,0.500000,1.570000,-2.680000,0.000000,7.160071,0.115412
+N45,0.054000,0.912000,0.500000,1.570000,-2.680000,0.000000,7.250398,0.064622
+N46,0.024000,0.900000,0.500000,1.570000,-2.680000,0.000000,7.360403,0.032311
+N47,0.006000,0.882000,0.500000,1.570000,-2.680000,0.000000,7.459839,0.025456

src/roboglue_ros/config/data/test06.data

@@ -0,0 +1,44 @@
+N,X,Y,Z,rR,rP,rY,DT,DS
+N0,0.024000,0.360000,0.500000,1.570000,-2.680000,0.000000,0.000000,0.000000
+N1,0.030000,0.354000,0.500000,1.570000,-2.680000,0.000000,0.101124,0.008485
+N2,0.054000,0.348000,0.500000,1.570000,-2.680000,0.000000,0.199747,0.024739
+N3,0.096000,0.348000,0.500000,1.570000,-2.680000,0.000000,0.300329,0.042000
+N4,0.126000,0.348000,0.500000,1.570000,-2.680000,0.000000,0.399742,0.030000
+N5,0.144000,0.354000,0.500000,1.570000,-2.680000,0.000000,0.500009,0.018974
+N6,0.162000,0.396000,0.500000,1.570000,-2.680000,0.000000,0.599898,0.045695
+N7,0.156000,0.456000,0.500000,1.570000,-2.680000,0.000000,0.700535,0.060299
+N8,0.132000,0.498000,0.500000,1.570000,-2.680000,0.000000,0.800986,0.048374
+N9,0.102000,0.510000,0.500000,1.570000,-2.680000,0.000000,0.899221,0.032311
+N10,0.060000,0.498000,0.500000,1.570000,-2.680000,0.000000,0.999973,0.043681
+N11,0.030000,0.456000,0.500000,1.570000,-2.680000,0.000000,1.099020,0.051614
+N12,0.012000,0.408000,0.500000,1.570000,-2.680000,0.000000,1.199482,0.051264
+N13,0.006000,0.372000,0.500000,1.570000,-2.680000,0.000000,1.299497,0.036497
+N14,-0.024000,0.372000,0.500000,1.570000,-2.680000,0.000000,1.399540,0.030000
+N15,-0.072000,0.372000,0.500000,1.570000,-2.680000,0.000000,1.499347,0.048000
+N16,-0.126000,0.384000,0.500000,1.570000,-2.680000,0.000000,1.599556,0.055317
+N17,-0.210000,0.438000,0.500000,1.570000,-2.680000,0.000000,1.729069,0.099860
+N18,-0.246000,0.462000,0.500000,1.570000,-2.680000,0.000000,1.799208,0.043267
+N19,-0.276000,0.480000,0.500000,1.570000,-2.680000,0.000000,1.899632,0.034986
+N20,-0.318000,0.492000,0.500000,1.570000,-2.680000,0.000000,2.000406,0.043681
+N21,-0.396000,0.516000,0.500000,1.570000,-2.680000,0.000000,2.099149,0.081609
+N22,-0.456000,0.588000,0.500000,1.570000,-2.680000,0.000000,2.199802,0.093723
+N23,-0.474000,0.612000,0.500000,1.570000,-2.680000,0.000000,2.299580,0.030000
+N24,-0.528000,0.612000,0.500000,1.570000,-2.680000,0.000000,2.401656,0.054000
+N25,-0.588000,0.570000,0.500000,1.570000,-2.680000,0.000000,2.500663,0.073239
+N26,-0.600000,0.510000,0.500000,1.570000,-2.680000,0.000000,2.599460,0.061188
+N27,-0.594000,0.468000,0.500000,1.570000,-2.680000,0.000000,2.699264,0.042426
+N28,-0.522000,0.402000,0.500000,1.570000,-2.680000,0.000000,2.799229,0.097673
+N29,-0.444000,0.384000,0.500000,1.570000,-2.680000,0.000000,2.899090,0.080050
+N30,-0.366000,0.378000,0.500000,1.570000,-2.680000,0.000000,2.999250,0.078230
+N31,-0.264000,0.402000,0.500000,1.570000,-2.680000,0.000000,3.099723,0.104785
+N32,-0.174000,0.456000,0.500000,1.570000,-2.680000,0.000000,3.200300,0.104957
+N33,-0.048000,0.540000,0.500000,1.570000,-2.680000,0.000000,3.299344,0.151433
+N34,0.012000,0.570000,0.500000,1.570000,-2.680000,0.000000,3.399485,0.067082
+N35,0.114000,0.594000,0.500000,1.570000,-2.680000,0.000000,3.499104,0.104785
+N36,0.198000,0.594000,0.500000,1.570000,-2.680000,0.000000,3.599649,0.084000
+N37,0.234000,0.594000,0.500000,1.570000,-2.680000,0.000000,4.139621,0.036000
+N38,0.588000,0.294000,0.500000,1.570000,-2.680000,0.000000,4.199079,0.464022
+N39,0.522000,0.246000,0.500000,1.570000,-2.680000,0.000000,4.299287,0.081609
+N40,0.426000,0.240000,0.500000,1.570000,-2.680000,0.000000,4.399122,0.096187
+N41,0.390000,0.252000,0.500000,1.570000,-2.680000,0.000000,4.500661,0.037947
+N42,0.336000,0.276000,0.500000,1.570000,-2.680000,0.000000,4.600375,0.059093

src/roboglue_ros/config/data/test07.data

@@ -0,0 +1,99 @@
+N,X,Y,Z,rR,rP,rY,DT,DS
+N0,0.222000,0.498000,0.500000,1.570000,-2.680000,0.000000,0.000000,0.000000
+N1,0.228000,0.498000,0.500000,1.570000,-2.680000,0.000000,0.099845,0.006000
+N2,0.270000,0.492000,0.500000,1.570000,-2.680000,0.000000,0.199288,0.042426
+N3,0.300000,0.480000,0.500000,1.570000,-2.680000,0.000000,0.299328,0.032311
+N4,0.342000,0.462000,0.500000,1.570000,-2.680000,0.000000,0.399793,0.045695
+N5,0.366000,0.462000,0.500000,1.570000,-2.680000,0.000000,0.499360,0.024000
+N6,0.396000,0.462000,0.500000,1.570000,-2.680000,0.000000,0.599444,0.030000
+N7,0.432000,0.504000,0.500000,1.570000,-2.680000,0.000000,0.699405,0.055317
+N8,0.432000,0.534000,0.500000,1.570000,-2.680000,0.000000,0.799390,0.030000
+N9,0.432000,0.564000,0.500000,1.570000,-2.680000,0.000000,0.899754,0.030000
+N10,0.420000,0.594000,0.500000,1.570000,-2.680000,0.000000,0.999404,0.032311
+N11,0.390000,0.636000,0.500000,1.570000,-2.680000,0.000000,1.100036,0.051614
+N12,0.336000,0.666000,0.500000,1.570000,-2.680000,0.000000,1.199527,0.061774
+N13,0.312000,0.672000,0.500000,1.570000,-2.680000,0.000000,1.309987,0.024739
+N14,0.276000,0.642000,0.500000,1.570000,-2.680000,0.000000,1.399715,0.046861
+N15,0.258000,0.618000,0.500000,1.570000,-2.680000,0.000000,1.499645,0.030000
+N16,0.222000,0.552000,0.500000,1.570000,-2.680000,0.000000,1.599721,0.075180
+N17,0.186000,0.492000,0.500000,1.570000,-2.680000,0.000000,1.699439,0.069971
+N18,0.156000,0.450000,0.500000,1.570000,-2.680000,0.000000,1.799817,0.051614
+N19,0.114000,0.414000,0.500000,1.570000,-2.680000,0.000000,1.899836,0.055317
+N20,0.078000,0.396000,0.500000,1.570000,-2.680000,0.000000,1.999562,0.040249
+N21,0.018000,0.372000,0.500000,1.570000,-2.680000,0.000000,2.099706,0.064622
+N22,-0.234000,0.306000,0.500000,1.570000,-2.680000,0.000000,2.631015,0.260500
+N23,-0.270000,0.306000,0.500000,1.570000,-2.680000,0.000000,2.699458,0.036000
+N24,-0.300000,0.294000,0.500000,1.570000,-2.680000,0.000000,2.799366,0.032311
+N25,-0.360000,0.282000,0.500000,1.570000,-2.680000,0.000000,2.899870,0.061188
+N26,-0.414000,0.264000,0.500000,1.570000,-2.680000,0.000000,3.000062,0.056921
+N27,-0.456000,0.258000,0.500000,1.570000,-2.680000,0.000000,3.099412,0.042426
+N28,-0.498000,0.246000,0.500000,1.570000,-2.680000,0.000000,3.199262,0.043681
+N29,-0.570000,0.228000,0.500000,1.570000,-2.680000,0.000000,3.299370,0.074216
+N30,-0.606000,0.228000,0.500000,1.570000,-2.680000,0.000000,3.399624,0.036000
+N31,-0.654000,0.222000,0.500000,1.570000,-2.680000,0.000000,3.499311,0.048374
+N32,-0.708000,0.210000,0.500000,1.570000,-2.680000,0.000000,3.599995,0.055317
+N33,-0.756000,0.204000,0.500000,1.570000,-2.680000,0.000000,3.699650,0.048374
+N34,-0.786000,0.204000,0.500000,1.570000,-2.680000,0.000000,3.800200,0.030000
+N35,-0.786000,0.240000,0.500000,1.570000,-2.680000,0.000000,3.899483,0.036000
+N36,-0.762000,0.294000,0.500000,1.570000,-2.680000,0.000000,3.999510,0.059093
+N37,-0.744000,0.330000,0.500000,1.570000,-2.680000,0.000000,4.099370,0.040249
+N38,-0.714000,0.390000,0.500000,1.570000,-2.680000,0.000000,4.199417,0.067082
+N39,-0.678000,0.468000,0.500000,1.570000,-2.680000,0.000000,4.299392,0.085907
+N40,-0.660000,0.522000,0.500000,1.570000,-2.680000,0.000000,4.399424,0.056921
+N41,-0.624000,0.582000,0.500000,1.570000,-2.680000,0.000000,4.499299,0.069971
+N42,-0.600000,0.618000,0.500000,1.570000,-2.680000,0.000000,4.600393,0.043267
+N43,-0.564000,0.666000,0.500000,1.570000,-2.680000,0.000000,4.699293,0.060000
+N44,-0.522000,0.720000,0.500000,1.570000,-2.680000,0.000000,4.799421,0.068411
+N45,-0.486000,0.768000,0.500000,1.570000,-2.680000,0.000000,4.899745,0.060000
+N46,-0.462000,0.792000,0.500000,1.570000,-2.680000,0.000000,4.999546,0.033941
+N47,-0.450000,0.804000,0.500000,1.570000,-2.680000,0.000000,5.100282,0.016971
+N48,-0.408000,0.750000,0.500000,1.570000,-2.680000,0.000000,5.200388,0.068411
+N49,-0.366000,0.672000,0.500000,1.570000,-2.680000,0.000000,5.299833,0.088589
+N50,-0.324000,0.618000,0.500000,1.570000,-2.680000,0.000000,5.399728,0.068411
+N51,-0.288000,0.570000,0.500000,1.570000,-2.680000,0.000000,5.499900,0.060000
+N52,-0.270000,0.546000,0.500000,1.570000,-2.680000,0.000000,5.599793,0.030000
+N53,-0.246000,0.504000,0.500000,1.570000,-2.680000,0.000000,5.700118,0.048374
+N54,-0.222000,0.468000,0.500000,1.570000,-2.680000,0.000000,5.799796,0.043267
+N55,-0.222000,0.450000,0.500000,1.570000,-2.680000,0.000000,5.900010,0.018000
+N56,-0.192000,0.468000,0.500000,1.570000,-2.680000,0.000000,6.099418,0.034986
+N57,-0.126000,0.540000,0.500000,1.570000,-2.680000,0.000000,6.199370,0.097673
+N58,-0.072000,0.600000,0.500000,1.570000,-2.680000,0.000000,6.299339,0.080722
+N59,-0.006000,0.678000,0.500000,1.570000,-2.680000,0.000000,6.399350,0.102176
+N60,0.036000,0.726000,0.500000,1.570000,-2.680000,0.000000,6.499390,0.063781
+N61,0.090000,0.774000,0.500000,1.570000,-2.680000,0.000000,6.599790,0.072250
+N62,0.132000,0.816000,0.500000,1.570000,-2.680000,0.000000,6.700395,0.059397
+N63,0.180000,0.870000,0.500000,1.570000,-2.680000,0.000000,6.799555,0.072250
+N64,0.210000,0.900000,0.500000,1.570000,-2.680000,0.000000,6.899435,0.042426
+N65,0.222000,0.894000,0.500000,1.570000,-2.680000,0.000000,6.999452,0.013416
+N66,0.300000,0.822000,0.500000,1.570000,-2.680000,0.000000,7.099575,0.106151
+N67,0.360000,0.756000,0.500000,1.570000,-2.680000,0.000000,7.199291,0.089196
+N68,0.372000,0.648000,0.500000,1.570000,-2.680000,0.000000,7.299513,0.108665
+N69,0.390000,0.576000,0.500000,1.570000,-2.680000,0.000000,7.399577,0.074216
+N70,0.408000,0.510000,0.500000,1.570000,-2.680000,0.000000,7.499828,0.068411
+N71,0.432000,0.456000,0.500000,1.570000,-2.680000,0.000000,7.599607,0.059093
+N72,0.456000,0.402000,0.500000,1.570000,-2.680000,0.000000,7.700205,0.059093
+N73,0.492000,0.348000,0.500000,1.570000,-2.680000,0.000000,7.799948,0.064900
+N74,0.516000,0.330000,0.500000,1.570000,-2.680000,0.000000,7.900597,0.030000
+N75,0.564000,0.318000,0.500000,1.570000,-2.680000,0.000000,7.999714,0.049477
+N76,0.642000,0.312000,0.500000,1.570000,-2.680000,0.000000,8.099455,0.078230
+N77,0.714000,0.324000,0.500000,1.570000,-2.680000,0.000000,8.199614,0.072993
+N78,0.762000,0.378000,0.500000,1.570000,-2.680000,0.000000,8.299397,0.072250
+N79,0.762000,0.462000,0.500000,1.570000,-2.680000,0.000000,8.399363,0.084000
+N80,0.738000,0.540000,0.500000,1.570000,-2.680000,0.000000,8.499330,0.081609
+N81,0.684000,0.588000,0.500000,1.570000,-2.680000,0.000000,8.599571,0.072250
+N82,0.642000,0.612000,0.500000,1.570000,-2.680000,0.000000,8.699286,0.048374
+N83,0.606000,0.624000,0.500000,1.570000,-2.680000,0.000000,8.800086,0.037947
+N84,0.564000,0.618000,0.500000,1.570000,-2.680000,0.000000,8.899911,0.042426
+N85,0.510000,0.582000,0.500000,1.570000,-2.680000,0.000000,9.000246,0.064900
+N86,0.450000,0.540000,0.500000,1.570000,-2.680000,0.000000,9.099615,0.073239
+N87,0.408000,0.504000,0.500000,1.570000,-2.680000,0.000000,9.199330,0.055317
+N88,0.342000,0.486000,0.500000,1.570000,-2.680000,0.000000,9.320244,0.068411
+N89,0.306000,0.480000,0.500000,1.570000,-2.680000,0.000000,9.399542,0.036497
+N90,0.222000,0.480000,0.500000,1.570000,-2.680000,0.000000,9.499833,0.084000
+N91,0.156000,0.468000,0.500000,1.570000,-2.680000,0.000000,9.599544,0.067082
+N92,0.102000,0.462000,0.500000,1.570000,-2.680000,0.000000,9.701046,0.054332
+N93,0.066000,0.456000,0.500000,1.570000,-2.680000,0.000000,9.900065,0.036497
+N94,0.030000,0.450000,0.500000,1.570000,-2.680000,0.000000,9.999311,0.036497
+N95,0.024000,0.450000,0.500000,1.570000,-2.680000,0.000000,10.100115,0.006000
+N96,0.012000,0.450000,0.500000,1.570000,-2.680000,0.000000,10.199828,0.012000
+N97,0.006000,0.444000,0.500000,1.570000,-2.680000,0.000000,10.299357,0.008485

src/roboglue_ros/config/data/test08.data

@@ -0,0 +1,105 @@
+N,X,Y,Z,rR,rP,rY,DT,DS
+N0,0.180000,0.462000,0.100000,1.570000,-2.680000,0.000000,0.000000,0.000000
+N1,0.156000,0.492000,0.100000,1.570000,-2.680000,0.000000,0.599476,0.038419
+N2,0.150000,0.492000,0.100000,1.570000,-2.680000,0.000000,0.699223,0.006000
+N3,0.144000,0.492000,0.100000,1.570000,-2.680000,0.000000,1.199189,0.006000
+N4,0.132000,0.498000,0.100000,1.570000,-2.680000,0.000000,1.298884,0.013416
+N5,0.132000,0.492000,0.100000,1.570000,-2.680000,0.000000,1.398902,0.006000
+N6,0.132000,0.504000,0.100000,1.570000,-2.680000,0.000000,1.498761,0.012000
+N7,0.138000,0.504000,0.100000,1.570000,-2.680000,0.000000,1.599445,0.006000
+N8,0.180000,0.474000,0.100000,1.570000,-2.680000,0.000000,1.698919,0.051614
+N9,0.252000,0.462000,0.100000,1.570000,-2.680000,0.000000,1.799533,0.072993
+N10,0.288000,0.456000,0.100000,1.570000,-2.680000,0.000000,1.899118,0.036497
+N11,0.306000,0.432000,0.200000,1.570000,-2.680000,0.000000,1.999268,0.104403
+N12,0.318000,0.432000,0.200000,1.570000,-2.680000,0.000000,2.099123,0.012000
+N13,0.348000,0.450000,0.200000,1.570000,-2.680000,0.000000,2.199304,0.034986
+N14,0.354000,0.474000,0.200000,1.570000,-2.680000,0.000000,2.299771,0.024739
+N15,0.354000,0.510000,0.200000,1.570000,-2.680000,0.000000,2.398789,0.036000
+N16,0.354000,0.528000,0.200000,1.570000,-2.680000,0.000000,2.498887,0.018000
+N17,0.354000,0.534000,0.200000,1.570000,-2.680000,0.000000,2.599302,0.006000
+N18,0.354000,0.546000,0.300000,1.570000,-2.680000,0.000000,2.699004,0.100717
+N19,0.342000,0.552000,0.300000,1.570000,-2.680000,0.000000,2.799432,0.013416
+N20,0.318000,0.570000,0.300000,1.570000,-2.680000,0.000000,2.898931,0.030000
+N21,0.312000,0.570000,0.300000,1.570000,-2.680000,0.000000,2.999049,0.006000
+N22,0.294000,0.582000,0.300000,1.570000,-2.680000,0.000000,3.099206,0.021633
+N23,0.288000,0.582000,0.300000,1.570000,-2.680000,0.000000,3.299108,0.006000
+N24,0.270000,0.594000,0.400000,1.570000,-2.680000,0.000000,3.399306,0.102313
+N25,0.216000,0.594000,0.400000,1.570000,-2.680000,0.000000,3.499260,0.054000
+N26,0.174000,0.594000,0.400000,1.570000,-2.680000,0.000000,3.598963,0.042000
+N27,0.126000,0.606000,0.400000,1.570000,-2.680000,0.000000,3.699027,0.049477
+N28,0.090000,0.594000,0.500000,1.570000,-2.680000,0.000000,3.799124,0.106958
+N29,0.060000,0.576000,0.500000,1.570000,-2.680000,0.000000,3.898934,0.034986
+N30,-0.012000,0.570000,0.500000,1.570000,-2.680000,0.000000,3.998990,0.072250
+N31,-0.084000,0.564000,0.500000,1.570000,-2.680000,0.000000,4.099445,0.072250
+N32,-0.126000,0.558000,0.500000,1.570000,-2.680000,0.000000,4.198995,0.042426
+N33,-0.192000,0.576000,0.500000,1.570000,-2.680000,0.000000,4.299522,0.068411
+N34,-0.240000,0.600000,0.500000,1.570000,-2.680000,0.000000,4.399250,0.053666
+N35,-0.300000,0.624000,0.600000,1.570000,-2.680000,0.000000,4.499130,0.119063
+N36,-0.372000,0.600000,0.600000,1.570000,-2.680000,0.000000,4.599456,0.075895
+N37,-0.438000,0.570000,0.600000,1.570000,-2.680000,0.000000,4.699610,0.072498
+N38,-0.504000,0.534000,0.600000,1.570000,-2.680000,0.000000,4.799245,0.075180
+N39,-0.534000,0.510000,0.600000,1.570000,-2.680000,0.000000,4.898898,0.038419
+N40,-0.564000,0.474000,0.600000,1.570000,-2.680000,0.000000,4.999048,0.046861
+N41,-0.606000,0.432000,0.600000,1.570000,-2.680000,0.000000,5.099518,0.059397
+N42,-0.642000,0.378000,0.600000,1.570000,-2.680000,0.000000,5.199459,0.064900
+N43,-0.660000,0.342000,0.600000,1.570000,-2.680000,0.000000,5.299089,0.040249
+N44,-0.702000,0.306000,0.600000,1.570000,-2.680000,0.000000,5.399258,0.055317
+N45,-0.762000,0.282000,0.700000,1.570000,-2.680000,0.000000,5.539069,0.119063
+N46,-0.786000,0.282000,0.700000,1.570000,-2.680000,0.000000,5.599049,0.024000
+N47,-0.792000,0.312000,0.700000,1.570000,-2.680000,0.000000,5.699624,0.030594
+N48,-0.762000,0.408000,0.700000,1.570000,-2.680000,0.000000,5.809004,0.100578
+N49,-0.726000,0.456000,0.700000,1.570000,-2.680000,0.000000,5.898953,0.060000
+N50,-0.708000,0.468000,0.700000,1.570000,-2.680000,0.000000,5.998918,0.021633
+N51,-0.684000,0.474000,0.700000,1.570000,-2.680000,0.000000,6.099024,0.024739
+N52,-0.624000,0.456000,0.700000,1.570000,-2.680000,0.000000,6.198985,0.062642
+N53,-0.564000,0.414000,0.700000,1.570000,-2.680000,0.000000,6.299409,0.073239
+N54,-0.534000,0.396000,0.600000,1.570000,-2.680000,0.000000,6.398868,0.105943
+N55,-0.480000,0.378000,0.600000,1.570000,-2.680000,0.000000,6.499070,0.056921
+N56,-0.420000,0.360000,0.600000,1.570000,-2.680000,0.000000,6.599106,0.062642
+N57,-0.354000,0.342000,0.600000,1.570000,-2.680000,0.000000,6.698989,0.068411
+N58,-0.282000,0.318000,0.600000,1.570000,-2.680000,0.000000,6.799195,0.075895
+N59,-0.216000,0.318000,0.600000,1.570000,-2.680000,0.000000,6.898923,0.066000
+N60,-0.138000,0.336000,0.600000,1.570000,-2.680000,0.000000,6.998935,0.080050
+N61,-0.084000,0.390000,0.600000,1.570000,-2.680000,0.000000,7.098875,0.076368
+N62,-0.060000,0.468000,0.500000,1.570000,-2.680000,0.000000,7.199195,0.129074
+N63,-0.060000,0.576000,0.500000,1.570000,-2.680000,0.000000,7.299093,0.108000
+N64,-0.060000,0.642000,0.500000,1.570000,-2.680000,0.000000,7.399011,0.066000
+N65,-0.054000,0.696000,0.500000,1.570000,-2.680000,0.000000,7.499206,0.054332
+N66,-0.048000,0.726000,0.500000,1.570000,-2.680000,0.000000,7.589040,0.030594
+N67,-0.030000,0.756000,0.500000,1.570000,-2.680000,0.000000,7.699190,0.034986
+N68,-0.024000,0.756000,0.500000,1.570000,-2.680000,0.000000,7.799188,0.006000
+N69,0.006000,0.738000,0.400000,1.570000,-2.680000,0.000000,7.899013,0.105943
+N70,0.024000,0.684000,0.400000,1.570000,-2.680000,0.000000,7.998955,0.056921
+N71,0.018000,0.606000,0.400000,1.570000,-2.680000,0.000000,8.099300,0.078230
+N72,0.018000,0.540000,0.400000,1.570000,-2.680000,0.000000,8.198857,0.066000
+N73,0.018000,0.504000,0.400000,1.570000,-2.680000,0.000000,8.298958,0.036000
+N74,0.024000,0.486000,0.400000,1.570000,-2.680000,0.000000,8.388918,0.018974
+N75,0.042000,0.468000,0.400000,1.570000,-2.680000,0.000000,8.498852,0.025456
+N76,0.048000,0.468000,0.300000,1.570000,-2.680000,0.000000,8.589102,0.100180
+N77,0.066000,0.468000,0.300000,1.570000,-2.680000,0.000000,8.699564,0.018000
+N78,0.084000,0.468000,0.300000,1.570000,-2.680000,0.000000,8.798934,0.018000
+N79,0.108000,0.486000,0.300000,1.570000,-2.680000,0.000000,8.899019,0.030000
+N80,0.132000,0.492000,0.300000,1.570000,-2.680000,0.000000,8.999140,0.024739
+N81,0.162000,0.492000,0.300000,1.570000,-2.680000,0.000000,9.099684,0.030000
+N82,0.198000,0.474000,0.300000,1.570000,-2.680000,0.000000,9.200492,0.040249
+N83,0.228000,0.432000,0.300000,1.570000,-2.680000,0.000000,9.299287,0.051614
+N84,0.240000,0.414000,0.300000,1.570000,-2.680000,0.000000,9.398992,0.021633
+N85,0.252000,0.402000,0.300000,1.570000,-2.680000,0.000000,9.491488,0.016971
+N86,0.258000,0.402000,0.200000,1.570000,-2.680000,0.000000,9.899575,0.100180
+N87,0.276000,0.420000,0.200000,1.570000,-2.680000,0.000000,9.998903,0.025456
+N88,0.288000,0.480000,0.200000,1.570000,-2.680000,0.000000,10.099272,0.061188
+N89,0.282000,0.528000,0.200000,1.570000,-2.680000,0.000000,10.198892,0.048374
+N90,0.258000,0.558000,0.200000,1.570000,-2.680000,0.000000,10.298834,0.038419
+N91,0.228000,0.570000,0.200000,1.570000,-2.680000,0.000000,10.398873,0.032311
+N92,0.174000,0.576000,0.300000,1.570000,-2.680000,0.000000,10.508978,0.113807
+N93,0.150000,0.576000,0.300000,1.570000,-2.680000,0.000000,10.598974,0.024000
+N94,0.102000,0.576000,0.300000,1.570000,-2.680000,0.000000,10.699043,0.048000
+N95,0.066000,0.576000,0.300000,1.570000,-2.680000,0.000000,10.799033,0.036000
+N96,0.048000,0.576000,0.300000,1.570000,-2.680000,0.000000,10.909003,0.018000
+N97,0.012000,0.564000,0.300000,1.570000,-2.680000,0.000000,10.999046,0.037947
+N98,-0.018000,0.564000,0.300000,1.570000,-2.680000,0.000000,11.098940,0.030000
+N99,-0.060000,0.564000,0.200000,1.570000,-2.680000,0.000000,11.198901,0.108462
+N100,-0.102000,0.564000,0.200000,1.570000,-2.680000,0.000000,11.299076,0.042000
+N101,-0.168000,0.564000,0.200000,1.570000,-2.680000,0.000000,11.399510,0.066000
+N102,-0.198000,0.564000,0.200000,1.570000,-2.680000,0.000000,11.498799,0.030000
+N103,-0.222000,0.552000,0.200000,1.570000,-2.680000,0.000000,11.598804,0.026833

src/roboglue_ros/config/data/test09.data

@@ -0,0 +1,271 @@
+N,X,Y,Z,rR,rP,rY,DT,DS
+N0,-0.222000,0.552000,0.200000,1.570000,-2.680000,0.000000,0.000000,0.000000
+N1,0.228000,0.504000,0.200000,1.570000,-2.680000,0.000000,0.789702,0.452553
+N2,0.264000,0.462000,0.200000,1.570000,-2.680000,0.000000,0.890234,0.055317
+N3,0.294000,0.408000,0.200000,1.570000,-2.680000,0.000000,0.990317,0.061774
+N4,0.330000,0.366000,0.200000,1.570000,-2.680000,0.000000,1.090206,0.055317
+N5,0.354000,0.354000,0.200000,1.570000,-2.680000,0.000000,1.189858,0.026833
+N6,0.402000,0.348000,0.200000,1.570000,-2.680000,0.000000,1.290149,0.048374
+N7,0.444000,0.348000,0.200000,1.570000,-2.680000,0.000000,1.389962,0.042000
+N8,0.462000,0.354000,0.200000,1.570000,-2.680000,0.000000,1.490430,0.018974
+N9,0.474000,0.354000,0.200000,1.570000,-2.680000,0.000000,1.789751,0.012000
+N10,0.486000,0.354000,0.200000,1.570000,-2.680000,0.000000,1.989669,0.012000
+N11,0.498000,0.354000,0.200000,1.570000,-2.680000,0.000000,2.089468,0.012000
+N12,0.546000,0.384000,0.200000,1.570000,-2.680000,0.000000,2.189632,0.056604
+N13,0.564000,0.402000,0.200000,1.570000,-2.680000,0.000000,2.290032,0.025456
+N14,0.588000,0.432000,0.200000,1.570000,-2.680000,0.000000,2.390489,0.038419
+N15,0.600000,0.450000,0.200000,1.570000,-2.680000,0.000000,2.489892,0.021633
+N16,0.606000,0.480000,0.200000,1.570000,-2.680000,0.000000,2.590100,0.030594
+N17,0.612000,0.504000,0.200000,1.570000,-2.680000,0.000000,2.689703,0.024739
+N18,0.606000,0.540000,0.200000,1.570000,-2.680000,0.000000,2.789797,0.036497
+N19,0.588000,0.558000,0.200000,1.570000,-2.680000,0.000000,2.890343,0.025456
+N20,0.564000,0.582000,0.200000,1.570000,-2.680000,0.000000,2.990159,0.033941
+N21,0.528000,0.612000,0.200000,1.570000,-2.680000,0.000000,3.089745,0.046861
+N22,0.504000,0.642000,0.200000,1.570000,-2.680000,0.000000,3.190585,0.038419
+N23,0.498000,0.654000,0.200000,1.570000,-2.680000,0.000000,3.289464,0.013416
+N24,0.480000,0.678000,0.200000,1.570000,-2.680000,0.000000,3.389812,0.030000
+N25,0.468000,0.696000,0.200000,1.570000,-2.680000,0.000000,3.500560,0.021633
+N26,0.450000,0.708000,0.200000,1.570000,-2.680000,0.000000,3.590264,0.021633
+N27,0.438000,0.726000,0.200000,1.570000,-2.680000,0.000000,3.689632,0.021633
+N28,0.414000,0.762000,0.300000,1.570000,-2.680000,0.000000,3.790418,0.108959
+N29,0.396000,0.780000,0.300000,1.570000,-2.680000,0.000000,3.890028,0.025456
+N30,0.378000,0.792000,0.300000,1.570000,-2.680000,0.000000,3.990534,0.021633
+N31,0.366000,0.804000,0.300000,1.570000,-2.680000,0.000000,4.090007,0.016971
+N32,0.336000,0.828000,0.300000,1.570000,-2.680000,0.000000,4.189926,0.038419
+N33,0.324000,0.828000,0.300000,1.570000,-2.680000,0.000000,4.290224,0.012000
+N34,0.312000,0.846000,0.300000,1.570000,-2.680000,0.000000,4.389781,0.021633
+N35,0.300000,0.846000,0.300000,1.570000,-2.680000,0.000000,4.490250,0.012000
+N36,0.282000,0.858000,0.300000,1.570000,-2.680000,0.000000,4.590023,0.021633
+N37,0.276000,0.858000,0.300000,1.570000,-2.680000,0.000000,4.689603,0.006000
+N38,0.258000,0.852000,0.400000,1.570000,-2.680000,0.000000,4.789591,0.101784
+N39,0.228000,0.804000,0.400000,1.570000,-2.680000,0.000000,4.889554,0.056604
+N40,0.216000,0.768000,0.400000,1.570000,-2.680000,0.000000,4.989971,0.037947
+N41,0.192000,0.732000,0.400000,1.570000,-2.680000,0.000000,5.089885,0.043267
+N42,0.180000,0.690000,0.400000,1.570000,-2.680000,0.000000,5.189658,0.043681
+N43,0.168000,0.660000,0.400000,1.570000,-2.680000,0.000000,5.289743,0.032311
+N44,0.150000,0.636000,0.400000,1.570000,-2.680000,0.000000,5.389684,0.030000
+N45,0.132000,0.618000,0.400000,1.570000,-2.680000,0.000000,5.490355,0.025456
+N46,0.102000,0.606000,0.400000,1.570000,-2.680000,0.000000,5.589551,0.032311
+N47,0.060000,0.618000,0.400000,1.570000,-2.680000,0.000000,5.690109,0.043681
+N48,0.000000,0.648000,0.400000,1.570000,-2.680000,0.000000,5.789709,0.067082
+N49,-0.030000,0.672000,0.400000,1.570000,-2.680000,0.000000,5.889528,0.038419
+N50,-0.042000,0.690000,0.500000,1.570000,-2.680000,0.000000,5.989605,0.102313
+N51,-0.066000,0.702000,0.500000,1.570000,-2.680000,0.000000,6.089560,0.026833
+N52,-0.096000,0.702000,0.500000,1.570000,-2.680000,0.000000,6.190006,0.030000
+N53,-0.144000,0.708000,0.500000,1.570000,-2.680000,0.000000,6.289713,0.048374
+N54,-0.192000,0.708000,0.500000,1.570000,-2.680000,0.000000,6.389987,0.048000
+N55,-0.222000,0.708000,0.500000,1.570000,-2.680000,0.000000,6.489539,0.030000
+N56,-0.246000,0.708000,0.500000,1.570000,-2.680000,0.000000,6.589623,0.024000
+N57,-0.300000,0.708000,0.500000,1.570000,-2.680000,0.000000,6.689607,0.054000
+N58,-0.336000,0.708000,0.500000,1.570000,-2.680000,0.000000,6.789740,0.036000
+N59,-0.384000,0.708000,0.600000,1.570000,-2.680000,0.000000,6.890030,0.110923
+N60,-0.438000,0.708000,0.600000,1.570000,-2.680000,0.000000,6.989541,0.054000
+N61,-0.486000,0.696000,0.600000,1.570000,-2.680000,0.000000,7.090065,0.049477
+N62,-0.558000,0.678000,0.600000,1.570000,-2.680000,0.000000,7.200125,0.074216
+N63,-0.576000,0.672000,0.600000,1.570000,-2.680000,0.000000,7.289994,0.018974
+N64,-0.600000,0.672000,0.600000,1.570000,-2.680000,0.000000,7.389734,0.024000
+N65,-0.624000,0.654000,0.600000,1.570000,-2.680000,0.000000,7.489808,0.030000
+N66,-0.654000,0.630000,0.600000,1.570000,-2.680000,0.000000,7.589723,0.038419
+N67,-0.672000,0.612000,0.600000,1.570000,-2.680000,0.000000,7.690111,0.025456
+N68,-0.696000,0.588000,0.600000,1.570000,-2.680000,0.000000,7.789813,0.033941
+N69,-0.714000,0.576000,0.600000,1.570000,-2.680000,0.000000,7.890322,0.021633
+N70,-0.732000,0.552000,0.600000,1.570000,-2.680000,0.000000,7.989933,0.030000
+N71,-0.744000,0.534000,0.600000,1.570000,-2.680000,0.000000,8.090728,0.021633
+N72,-0.762000,0.516000,0.600000,1.570000,-2.680000,0.000000,8.189870,0.025456
+N73,-0.792000,0.480000,0.600000,1.570000,-2.680000,0.000000,8.289584,0.046861
+N74,-0.804000,0.462000,0.600000,1.570000,-2.680000,0.000000,8.389998,0.021633
+N75,-0.816000,0.444000,0.700000,1.570000,-2.680000,0.000000,8.489571,0.102313
+N76,-0.822000,0.426000,0.700000,1.570000,-2.680000,0.000000,8.589586,0.018974
+N77,-0.828000,0.396000,0.700000,1.570000,-2.680000,0.000000,8.689524,0.030594
+N78,-0.846000,0.336000,0.700000,1.570000,-2.680000,0.000000,8.789796,0.062642
+N79,-0.858000,0.306000,0.700000,1.570000,-2.680000,0.000000,8.891267,0.032311
+N80,-0.858000,0.264000,0.700000,1.570000,-2.680000,0.000000,8.990013,0.042000
+N81,-0.858000,0.252000,0.700000,1.570000,-2.680000,0.000000,9.090256,0.012000
+N82,-0.840000,0.216000,0.700000,1.570000,-2.680000,0.000000,9.189635,0.040249
+N83,-0.816000,0.180000,0.700000,1.570000,-2.680000,0.000000,9.290023,0.043267
+N84,-0.804000,0.174000,0.700000,1.570000,-2.680000,0.000000,9.390877,0.013416
+N85,-0.762000,0.162000,0.700000,1.570000,-2.680000,0.000000,9.490599,0.043681
+N86,-0.726000,0.162000,0.700000,1.570000,-2.680000,0.000000,9.589981,0.036000
+N87,-0.696000,0.162000,0.700000,1.570000,-2.680000,0.000000,9.690553,0.030000
+N88,-0.678000,0.162000,0.700000,1.570000,-2.680000,0.000000,9.790101,0.018000
+N89,-0.630000,0.180000,0.700000,1.570000,-2.680000,0.000000,9.889606,0.051264
+N90,-0.600000,0.198000,0.700000,1.570000,-2.680000,0.000000,9.990078,0.034986
+N91,-0.576000,0.222000,0.700000,1.570000,-2.680000,0.000000,10.089630,0.033941
+N92,-0.558000,0.228000,0.700000,1.570000,-2.680000,0.000000,10.189715,0.018974
+N93,-0.552000,0.246000,0.700000,1.570000,-2.680000,0.000000,10.290283,0.018974
+N94,-0.522000,0.282000,0.700000,1.570000,-2.680000,0.000000,10.390081,0.046861
+N95,-0.504000,0.318000,0.800000,1.570000,-2.680000,0.000000,10.489536,0.107796
+N96,-0.486000,0.348000,0.800000,1.570000,-2.680000,0.000000,10.590285,0.034986
+N97,-0.456000,0.366000,0.800000,1.570000,-2.680000,0.000000,10.690047,0.034986
+N98,-0.444000,0.384000,0.800000,1.570000,-2.680000,0.000000,10.789802,0.021633
+N99,-0.414000,0.408000,0.800000,1.570000,-2.680000,0.000000,10.890632,0.038419
+N100,-0.366000,0.438000,0.800000,1.570000,-2.680000,0.000000,10.989908,0.056604
+N101,-0.336000,0.456000,0.800000,1.570000,-2.680000,0.000000,11.090031,0.034986
+N102,-0.300000,0.480000,0.800000,1.570000,-2.680000,0.000000,11.189824,0.043267
+N103,-0.258000,0.504000,0.800000,1.570000,-2.680000,0.000000,11.289851,0.048374
+N104,-0.222000,0.534000,0.800000,1.570000,-2.680000,0.000000,11.389950,0.046861
+N105,-0.162000,0.558000,0.800000,1.570000,-2.680000,0.000000,11.490917,0.064622
+N106,-0.114000,0.558000,0.800000,1.570000,-2.680000,0.000000,11.589703,0.048000
+N107,-0.078000,0.570000,0.800000,1.570000,-2.680000,0.000000,11.689860,0.037947
+N108,-0.048000,0.570000,0.700000,1.570000,-2.680000,0.000000,11.790212,0.104403
+N109,-0.030000,0.570000,0.700000,1.570000,-2.680000,0.000000,11.889862,0.018000
+N110,-0.006000,0.570000,0.700000,1.570000,-2.680000,0.000000,11.990054,0.024000
+N111,0.012000,0.570000,0.700000,1.570000,-2.680000,0.000000,12.089976,0.018000
+N112,0.024000,0.558000,0.700000,1.570000,-2.680000,0.000000,12.189796,0.016971
+N113,0.054000,0.552000,0.700000,1.570000,-2.680000,0.000000,12.289888,0.030594
+N114,0.066000,0.540000,0.700000,1.570000,-2.680000,0.000000,12.389988,0.016971
+N115,0.078000,0.528000,0.700000,1.570000,-2.680000,0.000000,12.489730,0.016971
+N116,0.102000,0.510000,0.700000,1.570000,-2.680000,0.000000,12.589833,0.030000
+N117,0.120000,0.492000,0.700000,1.570000,-2.680000,0.000000,12.689678,0.025456
+N118,0.132000,0.474000,0.800000,1.570000,-2.680000,0.000000,12.789904,0.102313
+N119,0.156000,0.444000,0.800000,1.570000,-2.680000,0.000000,12.890303,0.038419
+N120,0.162000,0.420000,0.800000,1.570000,-2.680000,0.000000,12.989773,0.024739
+N121,0.174000,0.408000,0.800000,1.570000,-2.680000,0.000000,13.091190,0.016971
+N122,0.180000,0.390000,0.800000,1.570000,-2.680000,0.000000,13.199980,0.018974
+N123,0.192000,0.348000,0.800000,1.570000,-2.680000,0.000000,13.290233,0.043681
+N124,0.210000,0.312000,0.800000,1.570000,-2.680000,0.000000,13.389660,0.040249
+N125,0.228000,0.282000,0.800000,1.570000,-2.680000,0.000000,13.489990,0.034986
+N126,0.234000,0.264000,0.800000,1.570000,-2.680000,0.000000,13.590042,0.018974
+N127,0.252000,0.252000,0.900000,1.570000,-2.680000,0.000000,13.690045,0.102313
+N128,0.282000,0.240000,0.900000,1.570000,-2.680000,0.000000,13.790068,0.032311
+N129,0.324000,0.234000,0.900000,1.570000,-2.680000,0.000000,13.889973,0.042426
+N130,0.354000,0.234000,0.900000,1.570000,-2.680000,0.000000,13.989569,0.030000
+N131,0.378000,0.246000,0.900000,1.570000,-2.680000,0.000000,14.090115,0.026833
+N132,0.426000,0.294000,0.900000,1.570000,-2.680000,0.000000,14.189973,0.067882
+N133,0.468000,0.306000,0.900000,1.570000,-2.680000,0.000000,14.290010,0.043681
+N134,0.522000,0.306000,1.000000,1.570000,-2.680000,0.000000,14.389840,0.113649
+N135,0.540000,0.306000,1.000000,1.570000,-2.680000,0.000000,14.490384,0.018000
+N136,0.564000,0.288000,1.000000,1.570000,-2.680000,0.000000,14.589761,0.030000
+N137,0.576000,0.270000,1.000000,1.570000,-2.680000,0.000000,14.690236,0.021633
+N138,0.588000,0.234000,1.000000,1.570000,-2.680000,0.000000,14.789840,0.037947
+N139,0.588000,0.204000,1.000000,1.570000,-2.680000,0.000000,14.890024,0.030000
+N140,0.588000,0.180000,1.000000,1.570000,-2.680000,0.000000,14.989853,0.024000
+N141,0.588000,0.156000,1.000000,1.570000,-2.680000,0.000000,15.089921,0.024000
+N142,0.588000,0.114000,1.000000,1.570000,-2.680000,0.000000,15.189849,0.042000
+N143,0.588000,0.078000,1.000000,1.570000,-2.680000,0.000000,15.289897,0.036000
+N144,0.588000,0.024000,1.000000,1.570000,-2.680000,0.000000,15.390032,0.054000
+N145,0.588000,-0.012000,1.000000,1.570000,-2.680000,0.000000,15.490301,0.036000
+N146,0.594000,-0.048000,1.000000,1.570000,-2.680000,0.000000,15.590317,0.036497
+N147,0.594000,-0.066000,1.000000,1.570000,-2.680000,0.000000,15.689845,0.018000
+N148,0.594000,-0.108000,1.000000,1.570000,-2.680000,0.000000,15.790228,0.042000
+N149,0.594000,-0.126000,1.000000,1.570000,-2.680000,0.000000,15.890248,0.018000
+N150,0.594000,-0.144000,1.000000,1.570000,-2.680000,0.000000,15.990177,0.018000
+N151,0.588000,-0.162000,1.000000,1.570000,-2.680000,0.000000,16.090109,0.018974
+N152,0.576000,-0.180000,0.900000,1.570000,-2.680000,0.000000,16.190029,0.102313
+N153,0.564000,-0.198000,0.900000,1.570000,-2.680000,0.000000,16.290133,0.021633
+N154,0.558000,-0.210000,0.900000,1.570000,-2.680000,0.000000,16.389664,0.013416
+N155,0.540000,-0.234000,0.900000,1.570000,-2.680000,0.000000,16.489698,0.030000
+N156,0.504000,-0.270000,0.900000,1.570000,-2.680000,0.000000,16.589578,0.050912
+N157,0.468000,-0.300000,0.900000,1.570000,-2.680000,0.000000,16.690211,0.046861
+N158,0.450000,-0.312000,0.900000,1.570000,-2.680000,0.000000,16.789828,0.021633
+N159,0.432000,-0.330000,0.900000,1.570000,-2.680000,0.000000,16.889634,0.025456
+N160,0.414000,-0.348000,0.800000,1.570000,-2.680000,0.000000,16.989703,0.103189
+N161,0.384000,-0.366000,0.800000,1.570000,-2.680000,0.000000,17.089802,0.034986
+N162,0.366000,-0.390000,0.800000,1.570000,-2.680000,0.000000,17.189761,0.030000
+N163,0.336000,-0.414000,0.800000,1.570000,-2.680000,0.000000,17.289978,0.038419
+N164,0.324000,-0.438000,0.800000,1.570000,-2.680000,0.000000,17.389887,0.026833
+N165,0.282000,-0.462000,0.800000,1.570000,-2.680000,0.000000,17.489965,0.048374
+N166,0.246000,-0.486000,0.800000,1.570000,-2.680000,0.000000,17.589597,0.043267
+N167,0.216000,-0.504000,0.800000,1.570000,-2.680000,0.000000,17.689649,0.034986
+N168,0.198000,-0.510000,0.800000,1.570000,-2.680000,0.000000,17.789941,0.018974
+N169,0.180000,-0.522000,0.800000,1.570000,-2.680000,0.000000,17.889659,0.021633
+N170,0.162000,-0.534000,0.800000,1.570000,-2.680000,0.000000,17.989798,0.021633
+N171,0.108000,-0.540000,0.700000,1.570000,-2.680000,0.000000,18.089752,0.113807
+N172,0.066000,-0.540000,0.700000,1.570000,-2.680000,0.000000,18.189916,0.042000
+N173,0.024000,-0.552000,0.700000,1.570000,-2.680000,0.000000,18.289936,0.043681
+N174,-0.006000,-0.552000,0.700000,1.570000,-2.680000,0.000000,18.389790,0.030000
+N175,-0.048000,-0.558000,0.700000,1.570000,-2.680000,0.000000,18.490140,0.042426
+N176,-0.090000,-0.558000,0.700000,1.570000,-2.680000,0.000000,18.589950,0.042000
+N177,-0.132000,-0.570000,0.700000,1.570000,-2.680000,0.000000,18.689986,0.043681
+N178,-0.156000,-0.570000,0.700000,1.570000,-2.680000,0.000000,18.789685,0.024000
+N179,-0.198000,-0.570000,0.700000,1.570000,-2.680000,0.000000,18.889978,0.042000
+N180,-0.222000,-0.564000,0.700000,1.570000,-2.680000,0.000000,18.989959,0.024739
+N181,-0.276000,-0.540000,0.700000,1.570000,-2.680000,0.000000,19.089811,0.059093
+N182,-0.330000,-0.486000,0.700000,1.570000,-2.680000,0.000000,19.189941,0.076368
+N183,-0.360000,-0.468000,0.700000,1.570000,-2.680000,0.000000,19.290368,0.034986
+N184,-0.390000,-0.468000,0.700000,1.570000,-2.680000,0.000000,19.390061,0.030000
+N185,-0.414000,-0.474000,0.700000,1.570000,-2.680000,0.000000,19.489604,0.024739
+N186,-0.432000,-0.516000,0.700000,1.570000,-2.680000,0.000000,19.589725,0.045695
+N187,-0.444000,-0.540000,0.700000,1.570000,-2.680000,0.000000,19.689928,0.026833
+N188,-0.444000,-0.570000,0.700000,1.570000,-2.680000,0.000000,19.790519,0.030000
+N189,-0.444000,-0.582000,0.700000,1.570000,-2.680000,0.000000,19.889919,0.012000
+N190,-0.432000,-0.618000,0.700000,1.570000,-2.680000,0.000000,19.989868,0.037947
+N191,-0.408000,-0.642000,0.700000,1.570000,-2.680000,0.000000,20.090282,0.033941
+N192,-0.372000,-0.678000,0.700000,1.570000,-2.680000,0.000000,20.189930,0.050912
+N193,-0.342000,-0.690000,0.700000,1.570000,-2.680000,0.000000,20.289603,0.032311
+N194,-0.324000,-0.702000,0.700000,1.570000,-2.680000,0.000000,20.390705,0.021633
+N195,-0.288000,-0.702000,0.700000,1.570000,-2.680000,0.000000,20.489908,0.036000
+N196,-0.216000,-0.702000,0.600000,1.570000,-2.680000,0.000000,20.589972,0.123223
+N197,-0.144000,-0.702000,0.600000,1.570000,-2.680000,0.000000,20.700503,0.072000
+N198,-0.066000,-0.696000,0.600000,1.570000,-2.680000,0.000000,20.789993,0.078230
+N199,-0.006000,-0.672000,0.600000,1.570000,-2.680000,0.000000,20.890241,0.064622
+N200,0.042000,-0.660000,0.600000,1.570000,-2.680000,0.000000,20.990226,0.049477
+N201,0.090000,-0.642000,0.600000,1.570000,-2.680000,0.000000,21.089922,0.051264
+N202,0.114000,-0.630000,0.600000,1.570000,-2.680000,0.000000,21.189826,0.026833
+N203,0.144000,-0.612000,0.600000,1.570000,-2.680000,0.000000,21.290215,0.034986
+N204,0.180000,-0.594000,0.600000,1.570000,-2.680000,0.000000,21.389700,0.040249
+N205,0.192000,-0.588000,0.600000,1.570000,-2.680000,0.000000,21.490312,0.013416
+N206,0.216000,-0.552000,0.700000,1.570000,-2.680000,0.000000,21.589868,0.108959
+N207,0.216000,-0.486000,0.700000,1.570000,-2.680000,0.000000,21.689975,0.066000
+N208,0.222000,-0.450000,0.700000,1.570000,-2.680000,0.000000,21.790130,0.036497
+N209,0.234000,-0.426000,0.700000,1.570000,-2.680000,0.000000,21.890054,0.026833
+N210,0.252000,-0.378000,0.700000,1.570000,-2.680000,0.000000,21.990618,0.051264
+N211,0.276000,-0.330000,0.700000,1.570000,-2.680000,0.000000,22.089592,0.053666
+N212,0.294000,-0.288000,0.700000,1.570000,-2.680000,0.000000,22.189731,0.045695
+N213,0.324000,-0.246000,0.700000,1.570000,-2.680000,0.000000,22.290270,0.051614
+N214,0.336000,-0.222000,0.700000,1.570000,-2.680000,0.000000,22.389861,0.026833
+N215,0.366000,-0.174000,0.700000,1.570000,-2.680000,0.000000,22.489594,0.056604
+N216,0.378000,-0.132000,0.700000,1.570000,-2.680000,0.000000,22.589768,0.043681
+N217,0.378000,-0.096000,0.700000,1.570000,-2.680000,0.000000,22.689914,0.036000
+N218,0.378000,-0.066000,0.700000,1.570000,-2.680000,0.000000,22.789832,0.030000
+N219,0.378000,-0.018000,0.700000,1.570000,-2.680000,0.000000,22.890051,0.048000
+N220,0.384000,0.012000,0.800000,1.570000,-2.680000,0.000000,22.989902,0.104575
+N221,0.390000,0.084000,0.800000,1.570000,-2.680000,0.000000,23.100836,0.072250
+N222,0.414000,0.126000,0.800000,1.570000,-2.680000,0.000000,23.189849,0.048374
+N223,0.438000,0.150000,0.800000,1.570000,-2.680000,0.000000,23.289685,0.033941
+N224,0.510000,0.168000,0.800000,1.570000,-2.680000,0.000000,23.389620,0.074216
+N225,0.552000,0.168000,0.800000,1.570000,-2.680000,0.000000,23.490127,0.042000
+N226,0.600000,0.162000,0.800000,1.570000,-2.680000,0.000000,23.590219,0.048374
+N227,0.618000,0.156000,0.800000,1.570000,-2.680000,0.000000,23.689759,0.018974
+N228,0.636000,0.156000,0.800000,1.570000,-2.680000,0.000000,23.789761,0.018000
+N229,0.672000,0.180000,0.900000,1.570000,-2.680000,0.000000,23.889626,0.108959
+N230,0.678000,0.192000,0.900000,1.570000,-2.680000,0.000000,23.989723,0.013416
+N231,0.684000,0.228000,0.900000,1.570000,-2.680000,0.000000,24.089809,0.036497
+N232,0.684000,0.276000,0.900000,1.570000,-2.680000,0.000000,24.250537,0.048000
+N233,0.684000,0.282000,0.900000,1.570000,-2.680000,0.000000,24.290145,0.006000
+N234,0.678000,0.324000,0.900000,1.570000,-2.680000,0.000000,24.390312,0.042426
+N235,0.660000,0.354000,0.900000,1.570000,-2.680000,0.000000,24.489770,0.034986
+N236,0.648000,0.372000,0.900000,1.570000,-2.680000,0.000000,24.589632,0.021633
+N237,0.618000,0.408000,0.900000,1.570000,-2.680000,0.000000,24.689632,0.046861
+N238,0.600000,0.426000,0.900000,1.570000,-2.680000,0.000000,24.789577,0.025456
+N239,0.582000,0.438000,0.900000,1.570000,-2.680000,0.000000,24.890340,0.021633
+N240,0.564000,0.456000,0.900000,1.570000,-2.680000,0.000000,24.989908,0.025456
+N241,0.552000,0.456000,0.900000,1.570000,-2.680000,0.000000,25.089644,0.012000
+N242,0.534000,0.468000,1.000000,1.570000,-2.680000,0.000000,25.190082,0.102313
+N243,0.480000,0.480000,1.000000,1.570000,-2.680000,0.000000,25.289750,0.055317
+N244,0.444000,0.480000,1.000000,1.570000,-2.680000,0.000000,25.389650,0.036000
+N245,0.402000,0.492000,1.000000,1.570000,-2.680000,0.000000,25.489607,0.043681
+N246,0.348000,0.510000,1.000000,1.570000,-2.680000,0.000000,25.589915,0.056921
+N247,0.318000,0.510000,1.000000,1.570000,-2.680000,0.000000,25.690704,0.030000
+N248,0.300000,0.510000,1.000000,1.570000,-2.680000,0.000000,25.790321,0.018000
+N249,0.282000,0.510000,1.000000,1.570000,-2.680000,0.000000,25.889689,0.018000
+N250,0.264000,0.510000,1.000000,1.570000,-2.680000,0.000000,25.989542,0.018000
+N251,0.246000,0.510000,1.000000,1.570000,-2.680000,0.000000,26.089719,0.018000
+N252,0.216000,0.504000,1.000000,1.570000,-2.680000,0.000000,26.190191,0.030594
+N253,0.180000,0.504000,1.000000,1.570000,-2.680000,0.000000,26.289565,0.036000
+N254,0.162000,0.504000,0.900000,1.570000,-2.680000,0.000000,26.389758,0.101607
+N255,0.132000,0.504000,0.900000,1.570000,-2.680000,0.000000,26.490543,0.030000
+N256,0.126000,0.504000,0.900000,1.570000,-2.680000,0.000000,26.590569,0.006000
+N257,0.102000,0.498000,0.900000,1.570000,-2.680000,0.000000,26.689666,0.024739
+N258,0.096000,0.498000,0.900000,1.570000,-2.680000,0.000000,26.989677,0.006000
+N259,0.096000,0.492000,0.800000,1.570000,-2.680000,0.000000,27.089877,0.100180
+N260,0.078000,0.486000,0.800000,1.570000,-2.680000,0.000000,27.189780,0.018974
+N261,0.066000,0.474000,0.800000,1.570000,-2.680000,0.000000,27.289703,0.016971
+N262,0.048000,0.468000,0.800000,1.570000,-2.680000,0.000000,27.390108,0.018974
+N263,0.018000,0.456000,0.800000,1.570000,-2.680000,0.000000,27.489679,0.032311
+N264,0.000000,0.456000,0.800000,1.570000,-2.680000,0.000000,27.589972,0.018000
+N265,-0.006000,0.456000,0.800000,1.570000,-2.680000,0.000000,27.690483,0.006000
+N266,-0.018000,0.462000,0.800000,1.570000,-2.680000,0.000000,27.789523,0.013416
+N267,-0.024000,0.474000,0.800000,1.570000,-2.680000,0.000000,27.889563,0.013416
+N268,-0.024000,0.486000,0.800000,1.570000,-2.680000,0.000000,27.989880,0.012000
+N269,-0.024000,0.492000,0.800000,1.570000,-2.680000,0.000000,28.189765,0.006000

src/roboglue_ros/config/data/test10.data

@@ -0,0 +1,281 @@
+N,X,Y,Z,rR,rP,rY,DT,DS
+N0,0.018000,0.468000,0.500000,1.570000,-2.680000,0.000000,0.000000,0.000000
+N1,0.066000,0.468000,0.500000,1.570000,-2.680000,0.000000,0.099399,0.048000
+N2,0.084000,0.468000,0.500000,1.570000,-2.680000,0.000000,0.199487,0.018000
+N3,0.102000,0.462000,0.500000,1.570000,-2.680000,0.000000,0.299232,0.018974
+N4,0.174000,0.468000,0.500000,1.570000,-2.680000,0.000000,0.399596,0.072250
+N5,0.222000,0.486000,0.500000,1.570000,-2.680000,0.000000,0.499247,0.051264
+N6,0.252000,0.492000,0.500000,1.570000,-2.680000,0.000000,0.599404,0.030594
+N7,0.264000,0.492000,0.500000,1.570000,-2.680000,0.000000,0.699401,0.012000
+N8,0.282000,0.492000,0.500000,1.570000,-2.680000,0.000000,0.809308,0.018000
+N9,0.336000,0.480000,0.500000,1.570000,-2.680000,0.000000,0.899991,0.055317
+N10,0.390000,0.450000,0.500000,1.570000,-2.680000,0.000000,0.999676,0.061774
+N11,0.414000,0.432000,0.500000,1.570000,-2.680000,0.000000,1.099306,0.030000
+N12,0.420000,0.432000,0.500000,1.570000,-2.680000,0.000000,1.199165,0.006000
+N13,0.462000,0.420000,0.500000,1.570000,-2.680000,0.000000,1.299475,0.043681
+N14,0.498000,0.396000,0.500000,1.570000,-2.680000,0.000000,1.399101,0.043267
+N15,0.516000,0.366000,0.500000,1.570000,-2.680000,0.000000,1.499117,0.034986
+N16,0.534000,0.342000,0.500000,1.570000,-2.680000,0.000000,1.599087,0.030000
+N17,0.546000,0.300000,0.500000,1.570000,-2.680000,0.000000,1.699387,0.043681
+N18,0.546000,0.288000,0.500000,1.570000,-2.680000,0.000000,1.799582,0.012000
+N19,0.558000,0.264000,0.600000,1.570000,-2.680000,0.000000,1.899840,0.103537
+N20,0.582000,0.252000,0.600000,1.570000,-2.680000,0.000000,1.999072,0.026833
+N21,0.606000,0.246000,0.600000,1.570000,-2.680000,0.000000,2.100209,0.024739
+N22,0.624000,0.264000,0.600000,1.570000,-2.680000,0.000000,2.199318,0.025456
+N23,0.636000,0.294000,0.600000,1.570000,-2.680000,0.000000,2.299813,0.032311
+N24,0.624000,0.342000,0.600000,1.570000,-2.680000,0.000000,2.399361,0.049477
+N25,0.588000,0.378000,0.600000,1.570000,-2.680000,0.000000,2.499658,0.050912
+N26,0.552000,0.414000,0.700000,1.570000,-2.680000,0.000000,2.599187,0.112214
+N27,0.528000,0.450000,0.700000,1.570000,-2.680000,0.000000,2.699669,0.043267
+N28,0.510000,0.474000,0.700000,1.570000,-2.680000,0.000000,2.799129,0.030000
+N29,0.492000,0.516000,0.700000,1.570000,-2.680000,0.000000,2.899234,0.045695
+N30,0.480000,0.540000,0.700000,1.570000,-2.680000,0.000000,2.999145,0.026833
+N31,0.456000,0.564000,0.700000,1.570000,-2.680000,0.000000,3.099142,0.033941
+N32,0.444000,0.576000,0.700000,1.570000,-2.680000,0.000000,3.199381,0.016971
+N33,0.426000,0.594000,0.700000,1.570000,-2.680000,0.000000,3.299039,0.025456
+N34,0.408000,0.612000,0.700000,1.570000,-2.680000,0.000000,3.399418,0.025456
+N35,0.378000,0.630000,0.600000,1.570000,-2.680000,0.000000,3.499024,0.105943
+N36,0.354000,0.642000,0.600000,1.570000,-2.680000,0.000000,3.599110,0.026833
+N37,0.318000,0.666000,0.600000,1.570000,-2.680000,0.000000,3.699041,0.043267
+N38,0.282000,0.684000,0.600000,1.570000,-2.680000,0.000000,3.799127,0.040249
+N39,0.186000,0.684000,0.600000,1.570000,-2.680000,0.000000,3.899328,0.096000
+N40,0.120000,0.690000,0.600000,1.570000,-2.680000,0.000000,4.009042,0.066272
+N41,0.036000,0.690000,0.600000,1.570000,-2.680000,0.000000,4.099215,0.084000
+N42,-0.042000,0.690000,0.600000,1.570000,-2.680000,0.000000,4.199061,0.078000
+N43,-0.060000,0.684000,0.600000,1.570000,-2.680000,0.000000,4.299351,0.018974
+N44,-0.072000,0.678000,0.600000,1.570000,-2.680000,0.000000,4.399945,0.013416
+N45,-0.084000,0.678000,0.600000,1.570000,-2.680000,0.000000,4.499371,0.012000
+N46,-0.126000,0.654000,0.500000,1.570000,-2.680000,0.000000,4.609093,0.111086
+N47,-0.192000,0.624000,0.500000,1.570000,-2.680000,0.000000,4.699277,0.072498
+N48,-0.288000,0.582000,0.500000,1.570000,-2.680000,0.000000,4.799605,0.104785
+N49,-0.342000,0.558000,0.500000,1.570000,-2.680000,0.000000,4.906907,0.059093
+N50,-0.420000,0.522000,0.500000,1.570000,-2.680000,0.000000,5.009728,0.085907
+N51,-0.474000,0.486000,0.500000,1.570000,-2.680000,0.000000,5.099057,0.064900
+N52,-0.504000,0.468000,0.500000,1.570000,-2.680000,0.000000,5.199238,0.034986
+N53,-0.534000,0.444000,0.500000,1.570000,-2.680000,0.000000,5.306677,0.038419
+N54,-0.564000,0.396000,0.500000,1.570000,-2.680000,0.000000,5.399572,0.056604
+N55,-0.582000,0.360000,0.500000,1.570000,-2.680000,0.000000,5.499882,0.040249
+N56,-0.588000,0.300000,0.500000,1.570000,-2.680000,0.000000,5.599502,0.060299
+N57,-0.588000,0.228000,0.500000,1.570000,-2.680000,0.000000,5.699937,0.072000
+N58,-0.588000,0.186000,0.500000,1.570000,-2.680000,0.000000,5.799358,0.042000
+N59,-0.588000,0.162000,0.500000,1.570000,-2.680000,0.000000,5.899185,0.024000
+N60,-0.600000,0.114000,0.500000,1.570000,-2.680000,0.000000,5.999681,0.049477
+N61,-0.618000,0.084000,0.500000,1.570000,-2.680000,0.000000,6.099146,0.034986
+N62,-0.654000,0.054000,0.500000,1.570000,-2.680000,0.000000,6.199197,0.046861
+N63,-0.684000,0.042000,0.500000,1.570000,-2.680000,0.000000,6.299295,0.032311
+N64,-0.684000,-0.006000,0.400000,1.570000,-2.680000,0.000000,6.399781,0.110923
+N65,-0.714000,-0.060000,0.400000,1.570000,-2.680000,0.000000,6.499531,0.061774
+N66,-0.720000,-0.078000,0.400000,1.570000,-2.680000,0.000000,6.599059,0.018974
+N67,-0.714000,-0.138000,0.400000,1.570000,-2.680000,0.000000,6.699166,0.060299
+N68,-0.684000,-0.174000,0.400000,1.570000,-2.680000,0.000000,6.799949,0.046861
+N69,-0.666000,-0.204000,0.400000,1.570000,-2.680000,0.000000,6.899083,0.034986
+N70,-0.630000,-0.246000,0.400000,1.570000,-2.680000,0.000000,6.999402,0.055317
+N71,-0.594000,-0.282000,0.400000,1.570000,-2.680000,0.000000,7.099029,0.050912
+N72,-0.564000,-0.306000,0.400000,1.570000,-2.680000,0.000000,7.199207,0.038419
+N73,-0.534000,-0.360000,0.400000,1.570000,-2.680000,0.000000,7.299419,0.061774
+N74,-0.510000,-0.402000,0.300000,1.570000,-2.680000,0.000000,7.409140,0.111086
+N75,-0.486000,-0.456000,0.300000,1.570000,-2.680000,0.000000,7.499152,0.059093
+N76,-0.444000,-0.534000,0.300000,1.570000,-2.680000,0.000000,7.599111,0.088589
+N77,-0.420000,-0.564000,0.300000,1.570000,-2.680000,0.000000,7.699038,0.038419
+N78,-0.390000,-0.588000,0.300000,1.570000,-2.680000,0.000000,7.799100,0.038419
+N79,-0.366000,-0.606000,0.300000,1.570000,-2.680000,0.000000,7.899315,0.030000
+N80,-0.318000,-0.618000,0.300000,1.570000,-2.680000,0.000000,7.999050,0.049477
+N81,-0.276000,-0.624000,0.300000,1.570000,-2.680000,0.000000,8.099194,0.042426
+N82,-0.246000,-0.630000,0.300000,1.570000,-2.680000,0.000000,8.199441,0.030594
+N83,-0.228000,-0.636000,0.400000,1.570000,-2.680000,0.000000,8.299074,0.101784
+N84,-0.186000,-0.642000,0.400000,1.570000,-2.680000,0.000000,8.400171,0.042426
+N85,-0.132000,-0.642000,0.400000,1.570000,-2.680000,0.000000,8.529155,0.054000
+N86,-0.108000,-0.654000,0.400000,1.570000,-2.680000,0.000000,8.609081,0.026833
+N87,-0.084000,-0.654000,0.400000,1.570000,-2.680000,0.000000,8.700496,0.024000
+N88,-0.054000,-0.654000,0.400000,1.570000,-2.680000,0.000000,8.799010,0.030000
+N89,-0.036000,-0.654000,0.400000,1.570000,-2.680000,0.000000,8.899076,0.018000
+N90,0.006000,-0.660000,0.400000,1.570000,-2.680000,0.000000,8.999114,0.042426
+N91,0.054000,-0.672000,0.500000,1.570000,-2.680000,0.000000,9.119357,0.111571
+N92,0.078000,-0.672000,0.500000,1.570000,-2.680000,0.000000,9.199369,0.024000
+N93,0.120000,-0.672000,0.500000,1.570000,-2.680000,0.000000,9.299097,0.042000
+N94,0.156000,-0.672000,0.500000,1.570000,-2.680000,0.000000,9.409070,0.036000
+N95,0.162000,-0.672000,0.500000,1.570000,-2.680000,0.000000,9.499174,0.006000
+N96,0.174000,-0.672000,0.500000,1.570000,-2.680000,0.000000,9.599311,0.012000
+N97,0.192000,-0.672000,0.500000,1.570000,-2.680000,0.000000,9.699135,0.018000
+N98,0.228000,-0.666000,0.600000,1.570000,-2.680000,0.000000,9.799712,0.106452
+N99,0.288000,-0.648000,0.600000,1.570000,-2.680000,0.000000,9.899310,0.062642
+N100,0.342000,-0.630000,0.600000,1.570000,-2.680000,0.000000,9.999240,0.056921
+N101,0.414000,-0.624000,0.600000,1.570000,-2.680000,0.000000,10.099163,0.072250
+N102,0.456000,-0.612000,0.600000,1.570000,-2.680000,0.000000,10.199866,0.043681
+N103,0.480000,-0.594000,0.600000,1.570000,-2.680000,0.000000,10.299138,0.030000
+N104,0.498000,-0.582000,0.700000,1.570000,-2.680000,0.000000,10.399590,0.102313
+N105,0.516000,-0.564000,0.700000,1.570000,-2.680000,0.000000,10.499244,0.025456
+N106,0.534000,-0.552000,0.700000,1.570000,-2.680000,0.000000,10.600033,0.021633
+N107,0.552000,-0.534000,0.700000,1.570000,-2.680000,0.000000,10.699097,0.025456
+N108,0.564000,-0.516000,0.700000,1.570000,-2.680000,0.000000,10.799206,0.021633
+N109,0.606000,-0.462000,0.700000,1.570000,-2.680000,0.000000,10.899244,0.068411
+N110,0.642000,-0.408000,0.800000,1.570000,-2.680000,0.000000,10.999588,0.119214
+N111,0.672000,-0.384000,0.800000,1.570000,-2.680000,0.000000,11.099823,0.038419
+N112,0.690000,-0.342000,0.800000,1.570000,-2.680000,0.000000,11.209120,0.045695
+N113,0.708000,-0.300000,0.800000,1.570000,-2.680000,0.000000,11.299404,0.045695
+N114,0.720000,-0.264000,0.800000,1.570000,-2.680000,0.000000,11.399410,0.037947
+N115,0.738000,-0.204000,0.800000,1.570000,-2.680000,0.000000,11.499335,0.062642
+N116,0.744000,-0.120000,0.800000,1.570000,-2.680000,0.000000,11.599080,0.084214
+N117,0.744000,-0.042000,0.900000,1.570000,-2.680000,0.000000,11.699714,0.126823
+N118,0.744000,-0.018000,0.900000,1.570000,-2.680000,0.000000,11.799216,0.024000
+N119,0.756000,0.066000,0.900000,1.570000,-2.680000,0.000000,11.899145,0.084853
+N120,0.756000,0.120000,0.900000,1.570000,-2.680000,0.000000,11.999277,0.054000
+N121,0.756000,0.174000,0.900000,1.570000,-2.680000,0.000000,12.100111,0.054000
+N122,0.756000,0.222000,0.900000,1.570000,-2.680000,0.000000,12.199107,0.048000
+N123,0.750000,0.258000,0.900000,1.570000,-2.680000,0.000000,12.299831,0.036497
+N124,0.750000,0.300000,1.000000,1.570000,-2.680000,0.000000,12.399347,0.108462
+N125,0.732000,0.342000,1.000000,1.570000,-2.680000,0.000000,12.499545,0.045695
+N126,0.714000,0.372000,1.000000,1.570000,-2.680000,0.000000,12.599452,0.034986
+N127,0.696000,0.408000,1.000000,1.570000,-2.680000,0.000000,12.699678,0.040249
+N128,0.678000,0.438000,1.000000,1.570000,-2.680000,0.000000,12.799349,0.034986
+N129,0.672000,0.456000,1.000000,1.570000,-2.680000,0.000000,12.899269,0.018974
+N130,0.642000,0.480000,1.000000,1.570000,-2.680000,0.000000,12.999264,0.038419
+N131,0.594000,0.510000,1.000000,1.570000,-2.680000,0.000000,13.099646,0.056604
+N132,0.546000,0.522000,1.000000,1.570000,-2.680000,0.000000,13.199188,0.049477
+N133,0.510000,0.534000,1.000000,1.570000,-2.680000,0.000000,13.299667,0.037947
+N134,0.492000,0.534000,1.000000,1.570000,-2.680000,0.000000,13.399574,0.018000
+N135,0.444000,0.540000,1.000000,1.570000,-2.680000,0.000000,13.499319,0.048374
+N136,0.402000,0.534000,0.900000,1.570000,-2.680000,0.000000,13.599166,0.108628
+N137,0.348000,0.492000,0.900000,1.570000,-2.680000,0.000000,13.699164,0.068411
+N138,0.312000,0.456000,0.900000,1.570000,-2.680000,0.000000,13.799297,0.050912
+N139,0.294000,0.426000,0.900000,1.570000,-2.680000,0.000000,13.899528,0.034986
+N140,0.282000,0.384000,0.900000,1.570000,-2.680000,0.000000,14.000186,0.043681
+N141,0.276000,0.348000,0.900000,1.570000,-2.680000,0.000000,14.099744,0.036497
+N142,0.258000,0.294000,0.900000,1.570000,-2.680000,0.000000,14.199093,0.056921
+N143,0.252000,0.264000,0.900000,1.570000,-2.680000,0.000000,14.299555,0.030594
+N144,0.216000,0.240000,0.900000,1.570000,-2.680000,0.000000,14.399216,0.043267
+N145,0.180000,0.240000,0.800000,1.570000,-2.680000,0.000000,14.499438,0.106283
+N146,0.144000,0.240000,0.800000,1.570000,-2.680000,0.000000,14.599081,0.036000
+N147,0.066000,0.270000,0.800000,1.570000,-2.680000,0.000000,14.699087,0.083570
+N148,0.042000,0.270000,0.800000,1.570000,-2.680000,0.000000,14.799361,0.024000
+N149,0.012000,0.282000,0.800000,1.570000,-2.680000,0.000000,14.900048,0.032311
+N150,-0.012000,0.300000,0.800000,1.570000,-2.680000,0.000000,14.999101,0.030000
+N151,-0.024000,0.312000,0.800000,1.570000,-2.680000,0.000000,15.099077,0.016971
+N152,-0.030000,0.342000,0.800000,1.570000,-2.680000,0.000000,15.199563,0.030594
+N153,-0.036000,0.360000,0.800000,1.570000,-2.680000,0.000000,15.299517,0.018974
+N154,-0.060000,0.402000,0.800000,1.570000,-2.680000,0.000000,15.399630,0.048374
+N155,-0.090000,0.438000,0.800000,1.570000,-2.680000,0.000000,15.499259,0.046861
+N156,-0.108000,0.462000,0.800000,1.570000,-2.680000,0.000000,15.599409,0.030000
+N157,-0.138000,0.480000,0.800000,1.570000,-2.680000,0.000000,15.700170,0.034986
+N158,-0.150000,0.504000,0.800000,1.570000,-2.680000,0.000000,15.799399,0.026833
+N159,-0.174000,0.510000,0.800000,1.570000,-2.680000,0.000000,15.899124,0.024739
+N160,-0.180000,0.522000,0.800000,1.570000,-2.680000,0.000000,15.999355,0.013416
+N161,-0.210000,0.540000,0.800000,1.570000,-2.680000,0.000000,16.099453,0.034986
+N162,-0.240000,0.540000,0.800000,1.570000,-2.680000,0.000000,16.199388,0.030000
+N163,-0.276000,0.540000,0.800000,1.570000,-2.680000,0.000000,16.299845,0.036000
+N164,-0.312000,0.540000,0.800000,1.570000,-2.680000,0.000000,16.399393,0.036000
+N165,-0.336000,0.534000,0.800000,1.570000,-2.680000,0.000000,16.499771,0.024739
+N166,-0.360000,0.522000,0.800000,1.570000,-2.680000,0.000000,16.599499,0.026833
+N167,-0.408000,0.498000,0.700000,1.570000,-2.680000,0.000000,16.699354,0.113490
+N168,-0.444000,0.468000,0.700000,1.570000,-2.680000,0.000000,16.799146,0.046861
+N169,-0.492000,0.378000,0.700000,1.570000,-2.680000,0.000000,16.899409,0.102000
+N170,-0.528000,0.300000,0.700000,1.570000,-2.680000,0.000000,16.999333,0.085907
+N171,-0.564000,0.252000,0.700000,1.570000,-2.680000,0.000000,17.099452,0.060000
+N172,-0.624000,0.240000,0.600000,1.570000,-2.680000,0.000000,17.199727,0.117235
+N173,-0.690000,0.306000,0.600000,1.570000,-2.680000,0.000000,17.299886,0.093338
+N174,-0.690000,0.378000,0.600000,1.570000,-2.680000,0.000000,17.399666,0.072000
+N175,-0.666000,0.420000,0.600000,1.570000,-2.680000,0.000000,17.499531,0.048374
+N176,-0.630000,0.468000,0.600000,1.570000,-2.680000,0.000000,17.599135,0.060000
+N177,-0.618000,0.480000,0.600000,1.570000,-2.680000,0.000000,17.699813,0.016971
+N178,-0.600000,0.492000,0.600000,1.570000,-2.680000,0.000000,17.809959,0.021633
+N179,-0.594000,0.498000,0.600000,1.570000,-2.680000,0.000000,17.899135,0.008485
+N180,-0.570000,0.510000,0.600000,1.570000,-2.680000,0.000000,17.999645,0.026833
+N181,-0.558000,0.516000,0.600000,1.570000,-2.680000,0.000000,18.099576,0.013416
+N182,-0.528000,0.516000,0.600000,1.570000,-2.680000,0.000000,18.199693,0.030000
+N183,-0.450000,0.516000,0.600000,1.570000,-2.680000,0.000000,18.299125,0.078000
+N184,-0.378000,0.516000,0.600000,1.570000,-2.680000,0.000000,18.399360,0.072000
+N185,-0.300000,0.516000,0.500000,1.570000,-2.680000,0.000000,18.499131,0.126823
+N186,-0.234000,0.510000,0.500000,1.570000,-2.680000,0.000000,18.599122,0.066272
+N187,-0.186000,0.498000,0.500000,1.570000,-2.680000,0.000000,18.699327,0.049477
+N188,-0.150000,0.486000,0.500000,1.570000,-2.680000,0.000000,18.799559,0.037947
+N189,-0.108000,0.486000,0.500000,1.570000,-2.680000,0.000000,18.900103,0.042000
+N190,-0.078000,0.474000,0.500000,1.570000,-2.680000,0.000000,19.000000,0.032311
+N191,-0.042000,0.456000,0.500000,1.570000,-2.680000,0.000000,19.099573,0.040249
+N192,-0.018000,0.438000,0.500000,1.570000,-2.680000,0.000000,19.199262,0.030000
+N193,0.018000,0.408000,0.400000,1.570000,-2.680000,0.000000,19.299352,0.110435
+N194,0.054000,0.378000,0.400000,1.570000,-2.680000,0.000000,19.399374,0.046861
+N195,0.072000,0.354000,0.400000,1.570000,-2.680000,0.000000,19.499406,0.030000
+N196,0.126000,0.300000,0.400000,1.570000,-2.680000,0.000000,19.599658,0.076368
+N197,0.156000,0.264000,0.400000,1.570000,-2.680000,0.000000,19.699745,0.046861
+N198,0.222000,0.216000,0.400000,1.570000,-2.680000,0.000000,19.799345,0.081609
+N199,0.252000,0.174000,0.400000,1.570000,-2.680000,0.000000,19.899820,0.051614
+N200,0.270000,0.150000,0.400000,1.570000,-2.680000,0.000000,19.999180,0.030000
+N201,0.300000,0.132000,0.400000,1.570000,-2.680000,0.000000,20.100150,0.034986
+N202,0.324000,0.126000,0.400000,1.570000,-2.680000,0.000000,20.199435,0.024739
+N203,0.342000,0.114000,0.400000,1.570000,-2.680000,0.000000,20.299434,0.021633
+N204,0.360000,0.096000,0.400000,1.570000,-2.680000,0.000000,20.399623,0.025456
+N205,0.384000,0.078000,0.400000,1.570000,-2.680000,0.000000,20.499127,0.030000
+N206,0.426000,0.042000,0.400000,1.570000,-2.680000,0.000000,20.599773,0.055317
+N207,0.444000,0.018000,0.400000,1.570000,-2.680000,0.000000,20.699187,0.030000
+N208,0.450000,0.006000,0.400000,1.570000,-2.680000,0.000000,20.799496,0.013416
+N209,0.450000,-0.012000,0.400000,1.570000,-2.680000,0.000000,20.899307,0.018000
+N210,0.444000,-0.066000,0.400000,1.570000,-2.680000,0.000000,20.999072,0.054332
+N211,0.444000,-0.102000,0.300000,1.570000,-2.680000,0.000000,21.099554,0.106283
+N212,0.444000,-0.138000,0.300000,1.570000,-2.680000,0.000000,21.199289,0.036000
+N213,0.426000,-0.186000,0.300000,1.570000,-2.680000,0.000000,21.299146,0.051264
+N214,0.420000,-0.222000,0.300000,1.570000,-2.680000,0.000000,21.399148,0.036497
+N215,0.402000,-0.276000,0.300000,1.570000,-2.680000,0.000000,21.499709,0.056921
+N216,0.402000,-0.324000,0.300000,1.570000,-2.680000,0.000000,21.599441,0.048000
+N217,0.390000,-0.360000,0.200000,1.570000,-2.680000,0.000000,21.699747,0.106958
+N218,0.372000,-0.384000,0.200000,1.570000,-2.680000,0.000000,21.799498,0.030000
+N219,0.342000,-0.408000,0.200000,1.570000,-2.680000,0.000000,21.899175,0.038419
+N220,0.282000,-0.408000,0.200000,1.570000,-2.680000,0.000000,21.999150,0.060000
+N221,0.228000,-0.426000,0.200000,1.570000,-2.680000,0.000000,22.099317,0.056921
+N222,0.216000,-0.432000,0.200000,1.570000,-2.680000,0.000000,22.199106,0.013416
+N223,0.192000,-0.444000,0.200000,1.570000,-2.680000,0.000000,22.299346,0.026833
+N224,0.150000,-0.468000,0.200000,1.570000,-2.680000,0.000000,22.399766,0.048374
+N225,0.144000,-0.474000,0.200000,1.570000,-2.680000,0.000000,22.499290,0.008485
+N226,0.102000,-0.498000,0.100000,1.570000,-2.680000,0.000000,22.599615,0.111086
+N227,0.084000,-0.516000,0.100000,1.570000,-2.680000,0.000000,22.699495,0.025456
+N228,0.078000,-0.534000,0.100000,1.570000,-2.680000,0.000000,22.799103,0.018974
+N229,0.060000,-0.546000,0.100000,1.570000,-2.680000,0.000000,22.899435,0.021633
+N230,0.048000,-0.552000,0.100000,1.570000,-2.680000,0.000000,22.999117,0.013416
+N231,0.030000,-0.552000,0.100000,1.570000,-2.680000,0.000000,23.099313,0.018000
+N232,0.000000,-0.558000,0.100000,1.570000,-2.680000,0.000000,23.199157,0.030594
+N233,-0.018000,-0.564000,0.100000,1.570000,-2.680000,0.000000,23.299565,0.018974
+N234,-0.054000,-0.570000,0.100000,1.570000,-2.680000,0.000000,23.407258,0.036497
+N235,-0.126000,-0.576000,0.000000,1.570000,-2.680000,0.000000,23.499231,0.123369
+N236,-0.132000,-0.588000,0.000000,1.570000,-2.680000,0.000000,23.599288,0.013416
+N237,-0.114000,-0.600000,0.000000,1.570000,-2.680000,0.000000,23.699941,0.021633
+N238,-0.048000,-0.612000,0.000000,1.570000,-2.680000,0.000000,23.799441,0.067082
+N239,-0.006000,-0.612000,0.000000,1.570000,-2.680000,0.000000,23.899627,0.042000
+N240,0.030000,-0.612000,0.000000,1.570000,-2.680000,0.000000,23.999424,0.036000
+N241,0.078000,-0.612000,0.000000,1.570000,-2.680000,0.000000,24.100018,0.048000
+N242,0.114000,-0.612000,0.000000,1.570000,-2.680000,0.000000,24.199256,0.036000
+N243,0.180000,-0.588000,0.000000,1.570000,-2.680000,0.000000,24.300292,0.070228
+N244,0.240000,-0.558000,0.000000,1.570000,-2.680000,0.000000,24.399245,0.067082
+N245,0.276000,-0.540000,0.000000,1.570000,-2.680000,0.000000,24.499623,0.040249
+N246,0.330000,-0.504000,0.100000,1.570000,-2.680000,0.000000,24.599727,0.119214
+N247,0.384000,-0.456000,0.100000,1.570000,-2.680000,0.000000,24.699228,0.072250
+N248,0.432000,-0.420000,0.100000,1.570000,-2.680000,0.000000,24.799660,0.060000
+N249,0.468000,-0.378000,0.100000,1.570000,-2.680000,0.000000,24.899643,0.055317
+N250,0.486000,-0.360000,0.100000,1.570000,-2.680000,0.000000,24.999272,0.025456
+N251,0.516000,-0.342000,0.100000,1.570000,-2.680000,0.000000,25.099173,0.034986
+N252,0.528000,-0.324000,0.100000,1.570000,-2.680000,0.000000,25.199157,0.021633
+N253,0.552000,-0.282000,0.100000,1.570000,-2.680000,0.000000,25.299481,0.048374
+N254,0.582000,-0.204000,0.100000,1.570000,-2.680000,0.000000,25.399234,0.083570
+N255,0.600000,-0.138000,0.200000,1.570000,-2.680000,0.000000,25.499147,0.121161
+N256,0.612000,-0.036000,0.200000,1.570000,-2.680000,0.000000,25.599123,0.102703
+N257,0.612000,0.036000,0.200000,1.570000,-2.680000,0.000000,25.699117,0.072000
+N258,0.612000,0.090000,0.200000,1.570000,-2.680000,0.000000,25.799148,0.054000
+N259,0.612000,0.132000,0.200000,1.570000,-2.680000,0.000000,25.899363,0.042000
+N260,0.612000,0.150000,0.200000,1.570000,-2.680000,0.000000,25.999470,0.018000
+N261,0.612000,0.174000,0.200000,1.570000,-2.680000,0.000000,26.099457,0.024000
+N262,0.582000,0.234000,0.300000,1.570000,-2.680000,0.000000,26.199434,0.120416
+N263,0.504000,0.306000,0.300000,1.570000,-2.680000,0.000000,26.299199,0.106151
+N264,0.450000,0.324000,0.300000,1.570000,-2.680000,0.000000,26.399226,0.056921
+N265,0.420000,0.342000,0.300000,1.570000,-2.680000,0.000000,26.499093,0.034986
+N266,0.384000,0.360000,0.300000,1.570000,-2.680000,0.000000,26.599330,0.040249
+N267,0.354000,0.378000,0.300000,1.570000,-2.680000,0.000000,26.699198,0.034986
+N268,0.312000,0.402000,0.300000,1.570000,-2.680000,0.000000,26.799408,0.048374
+N269,0.282000,0.414000,0.400000,1.570000,-2.680000,0.000000,26.899590,0.105090
+N270,0.234000,0.432000,0.400000,1.570000,-2.680000,0.000000,26.999392,0.051264
+N271,0.216000,0.432000,0.400000,1.570000,-2.680000,0.000000,27.100028,0.018000
+N272,0.186000,0.444000,0.400000,1.570000,-2.680000,0.000000,27.199156,0.032311
+N273,0.156000,0.450000,0.400000,1.570000,-2.680000,0.000000,27.299056,0.030594
+N274,0.120000,0.456000,0.400000,1.570000,-2.680000,0.000000,27.399840,0.036497
+N275,0.102000,0.462000,0.400000,1.570000,-2.680000,0.000000,27.509958,0.018974
+N276,0.072000,0.462000,0.400000,1.570000,-2.680000,0.000000,27.599743,0.030000
+N277,0.048000,0.468000,0.500000,1.570000,-2.680000,0.000000,27.699214,0.103015
+N278,0.018000,0.474000,0.500000,1.570000,-2.680000,0.000000,27.799255,0.030594
+N279,0.012000,0.474000,0.500000,1.570000,-2.680000,0.000000,27.899257,0.006000

src/roboglue_ros/config/data/test11.data

@@ -0,0 +1,187 @@
+N,X,Y,Z,rR,rP,rY,DT,DS
+N0,-0.024000,0.468000,0.500000,1.570000,-2.680000,0.000000,0.000000,0.000000
+N1,0.024000,0.468000,0.500000,1.570000,-2.680000,0.000000,0.110026,0.048000
+N2,0.084000,0.462000,0.500000,1.570000,-2.680000,0.000000,0.220103,0.060299
+N3,0.114000,0.450000,0.500000,1.570000,-2.680000,0.000000,0.299942,0.032311
+N4,0.150000,0.438000,0.500000,1.570000,-2.680000,0.000000,0.400133,0.037947
+N5,0.222000,0.414000,0.500000,1.570000,-2.680000,0.000000,0.500010,0.075895
+N6,0.264000,0.396000,0.500000,1.570000,-2.680000,0.000000,0.599897,0.045695
+N7,0.288000,0.372000,0.500000,1.570000,-2.680000,0.000000,0.700009,0.033941
+N8,0.306000,0.342000,0.500000,1.570000,-2.680000,0.000000,0.800035,0.034986
+N9,0.342000,0.312000,0.500000,1.570000,-2.680000,0.000000,0.900223,0.046861
+N10,0.372000,0.288000,0.500000,1.570000,-2.680000,0.000000,1.010142,0.038419
+N11,0.390000,0.270000,0.500000,1.570000,-2.680000,0.000000,1.100349,0.025456
+N12,0.438000,0.228000,0.500000,1.570000,-2.680000,0.000000,1.199981,0.063781
+N13,0.462000,0.198000,0.500000,1.570000,-2.680000,0.000000,1.300032,0.038419
+N14,0.492000,0.174000,0.500000,1.570000,-2.680000,0.000000,1.400020,0.038419
+N15,0.540000,0.162000,0.500000,1.570000,-2.680000,0.000000,1.508519,0.049477
+N16,0.576000,0.096000,0.500000,1.570000,-2.680000,0.000000,1.600145,0.075180
+N17,0.576000,0.030000,0.500000,1.570000,-2.680000,0.000000,1.700555,0.066000
+N18,0.570000,-0.042000,0.500000,1.570000,-2.680000,0.000000,1.810281,0.072250
+N19,0.546000,-0.114000,0.500000,1.570000,-2.680000,0.000000,1.910026,0.075895
+N20,0.522000,-0.174000,0.500000,1.570000,-2.680000,0.000000,2.000700,0.064622
+N21,0.522000,-0.258000,0.500000,1.570000,-2.680000,0.000000,2.100231,0.084000
+N22,0.522000,-0.318000,0.500000,1.570000,-2.680000,0.000000,2.200231,0.060000
+N23,0.522000,-0.384000,0.500000,1.570000,-2.680000,0.000000,2.300098,0.066000
+N24,0.522000,-0.456000,0.500000,1.570000,-2.680000,0.000000,2.400169,0.072000
+N25,0.510000,-0.504000,0.500000,1.570000,-2.680000,0.000000,2.500266,0.049477
+N26,0.492000,-0.564000,0.500000,1.570000,-2.680000,0.000000,2.600538,0.062642
+N27,0.468000,-0.612000,0.500000,1.570000,-2.680000,0.000000,2.699918,0.053666
+N28,0.432000,-0.684000,0.500000,1.570000,-2.680000,0.000000,2.810101,0.080498
+N29,0.390000,-0.738000,0.500000,1.570000,-2.680000,0.000000,2.910144,0.068411
+N30,0.336000,-0.792000,0.500000,1.570000,-2.680000,0.000000,3.000208,0.076368
+N31,0.288000,-0.816000,0.500000,1.570000,-2.680000,0.000000,3.099990,0.053666
+N32,0.216000,-0.852000,0.500000,1.570000,-2.680000,0.000000,3.200029,0.080498
+N33,0.156000,-0.876000,0.500000,1.570000,-2.680000,0.000000,3.300318,0.064622
+N34,0.084000,-0.894000,0.500000,1.570000,-2.680000,0.000000,3.400296,0.074216
+N35,0.006000,-0.894000,0.500000,1.570000,-2.680000,0.000000,3.510084,0.078000
+N36,-0.030000,-0.894000,0.500000,1.570000,-2.680000,0.000000,3.600752,0.036000
+N37,-0.114000,-0.858000,0.500000,1.570000,-2.680000,0.000000,3.710434,0.091389
+N38,-0.186000,-0.792000,0.500000,1.570000,-2.680000,0.000000,3.810175,0.097673
+N39,-0.282000,-0.726000,0.500000,1.570000,-2.680000,0.000000,3.900285,0.116499
+N40,-0.324000,-0.708000,0.500000,1.570000,-2.680000,0.000000,4.000680,0.045695
+N41,-0.360000,-0.720000,0.500000,1.570000,-2.680000,0.000000,4.131050,0.037947
+N42,-0.372000,-0.732000,0.500000,1.570000,-2.680000,0.000000,4.210272,0.016971
+N43,-0.378000,-0.804000,0.500000,1.570000,-2.680000,0.000000,4.300073,0.072250
+N44,-0.378000,-0.852000,0.500000,1.570000,-2.680000,0.000000,4.400084,0.048000
+N45,-0.348000,-0.906000,0.500000,1.570000,-2.680000,0.000000,4.500412,0.061774
+N46,-0.282000,-0.966000,0.500000,1.570000,-2.680000,0.000000,4.610294,0.089196
+N47,-0.216000,-1.008000,0.500000,1.570000,-2.680000,0.000000,4.700430,0.078230
+N48,-0.150000,-1.032000,0.500000,1.570000,-2.680000,0.000000,4.800371,0.070228
+N49,-0.084000,-1.032000,0.500000,1.570000,-2.680000,0.000000,4.899953,0.066000
+N50,-0.018000,-1.032000,0.500000,1.570000,-2.680000,0.000000,5.000526,0.066000
+N51,0.060000,-1.032000,0.500000,1.570000,-2.680000,0.000000,5.100206,0.078000
+N52,0.156000,-1.014000,0.500000,1.570000,-2.680000,0.000000,5.200356,0.097673
+N53,0.222000,-0.942000,0.500000,1.570000,-2.680000,0.000000,5.310917,0.097673
+N54,0.252000,-0.840000,0.500000,1.570000,-2.680000,0.000000,5.410048,0.106320
+N55,0.240000,-0.738000,0.500000,1.570000,-2.680000,0.000000,5.510769,0.102703
+N56,0.162000,-0.648000,0.500000,1.570000,-2.680000,0.000000,5.610153,0.119097
+N57,0.066000,-0.618000,0.500000,1.570000,-2.680000,0.000000,5.700175,0.100578
+N58,0.000000,-0.618000,0.500000,1.570000,-2.680000,0.000000,5.800410,0.066000
+N59,-0.060000,-0.624000,0.500000,1.570000,-2.680000,0.000000,5.900148,0.060299
+N60,-0.114000,-0.654000,0.500000,1.570000,-2.680000,0.000000,6.000507,0.061774
+N61,-0.144000,-0.702000,0.500000,1.570000,-2.680000,0.000000,6.100359,0.056604
+N62,-0.156000,-0.774000,0.500000,1.570000,-2.680000,0.000000,6.200530,0.072993
+N63,-0.108000,-0.858000,0.500000,1.570000,-2.680000,0.000000,6.300260,0.096747
+N64,-0.054000,-0.912000,0.500000,1.570000,-2.680000,0.000000,6.400414,0.076368
+N65,0.024000,-0.942000,0.500000,1.570000,-2.680000,0.000000,6.510222,0.083570
+N66,0.114000,-0.954000,0.500000,1.570000,-2.680000,0.000000,6.600056,0.090796
+N67,0.168000,-0.948000,0.500000,1.570000,-2.680000,0.000000,6.700104,0.054332
+N68,0.228000,-0.840000,0.500000,1.570000,-2.680000,0.000000,6.800106,0.123548
+N69,0.204000,-0.744000,0.500000,1.570000,-2.680000,0.000000,6.901071,0.098955
+N70,0.162000,-0.678000,0.500000,1.570000,-2.680000,0.000000,7.000386,0.078230
+N71,0.114000,-0.660000,0.500000,1.570000,-2.680000,0.000000,7.100571,0.051264
+N72,0.036000,-0.672000,0.500000,1.570000,-2.680000,0.000000,7.200365,0.078918
+N73,-0.030000,-0.702000,0.500000,1.570000,-2.680000,0.000000,7.310129,0.072498
+N74,-0.120000,-0.702000,0.500000,1.570000,-2.680000,0.000000,7.410015,0.090000
+N75,-0.204000,-0.702000,0.500000,1.570000,-2.680000,0.000000,7.500615,0.084000
+N76,-0.324000,-0.702000,0.500000,1.570000,-2.680000,0.000000,7.600154,0.120000
+N77,-0.576000,-0.678000,0.500000,1.570000,-2.680000,0.000000,7.700550,0.253140
+N78,-0.768000,-0.564000,0.500000,1.570000,-2.680000,0.000000,7.800451,0.223294
+N79,-0.840000,-0.366000,0.500000,1.570000,-2.680000,0.000000,7.900041,0.210685
+N80,-0.822000,-0.270000,0.500000,1.570000,-2.680000,0.000000,8.000734,0.097673
+N81,-0.780000,-0.216000,0.500000,1.570000,-2.680000,0.000000,8.110888,0.068411
+N82,-0.756000,-0.204000,0.500000,1.570000,-2.680000,0.000000,8.200177,0.026833
+N83,-0.720000,-0.204000,0.500000,1.570000,-2.680000,0.000000,8.310032,0.036000
+N84,-0.678000,-0.228000,0.500000,1.570000,-2.680000,0.000000,8.400046,0.048374
+N85,-0.642000,-0.282000,0.500000,1.570000,-2.680000,0.000000,8.500643,0.064900
+N86,-0.624000,-0.324000,0.500000,1.570000,-2.680000,0.000000,8.600497,0.045695
+N87,-0.624000,-0.366000,0.500000,1.570000,-2.680000,0.000000,8.700423,0.042000
+N88,-0.642000,-0.402000,0.500000,1.570000,-2.680000,0.000000,8.810334,0.040249
+N89,-0.690000,-0.432000,0.500000,1.570000,-2.680000,0.000000,8.921077,0.056604
+N90,-0.744000,-0.438000,0.500000,1.570000,-2.680000,0.000000,9.010261,0.054332
+N91,-0.780000,-0.438000,0.500000,1.570000,-2.680000,0.000000,9.110128,0.036000
+N92,-0.834000,-0.378000,0.500000,1.570000,-2.680000,0.000000,9.210067,0.080722
+N93,-0.882000,-0.282000,0.500000,1.570000,-2.680000,0.000000,9.300338,0.107331
+N94,-0.882000,-0.192000,0.500000,1.570000,-2.680000,0.000000,9.400162,0.090000
+N95,-0.858000,-0.108000,0.500000,1.570000,-2.680000,0.000000,9.510212,0.087361
+N96,-0.822000,-0.048000,0.500000,1.570000,-2.680000,0.000000,9.600315,0.069971
+N97,-0.786000,-0.018000,0.500000,1.570000,-2.680000,0.000000,9.700280,0.046861
+N98,-0.720000,-0.012000,0.500000,1.570000,-2.680000,0.000000,9.810091,0.066272
+N99,-0.636000,-0.060000,0.500000,1.570000,-2.680000,0.000000,9.910149,0.096747
+N100,-0.582000,-0.156000,0.500000,1.570000,-2.680000,0.000000,10.010215,0.110145
+N101,-0.588000,-0.240000,0.500000,1.570000,-2.680000,0.000000,10.110697,0.084214
+N102,-0.612000,-0.270000,0.500000,1.570000,-2.680000,0.000000,10.200004,0.038419
+N103,-0.666000,-0.276000,0.500000,1.570000,-2.680000,0.000000,10.300507,0.054332
+N104,-0.720000,-0.246000,0.500000,1.570000,-2.680000,0.000000,10.400037,0.061774
+N105,-0.786000,-0.168000,0.500000,1.570000,-2.680000,0.000000,10.500088,0.102176
+N106,-0.822000,-0.102000,0.500000,1.570000,-2.680000,0.000000,10.610010,0.075180
+N107,-0.792000,-0.000000,0.500000,1.570000,-2.680000,0.000000,10.700093,0.106320
+N108,-0.756000,0.060000,0.500000,1.570000,-2.680000,0.000000,10.800223,0.069971
+N109,-0.714000,0.126000,0.500000,1.570000,-2.680000,0.000000,10.910381,0.078230
+N110,-0.702000,0.126000,0.500000,1.570000,-2.680000,0.000000,11.000884,0.012000
+N111,-0.684000,0.144000,0.500000,1.570000,-2.680000,0.000000,11.101354,0.025456
+N112,-0.666000,0.162000,0.500000,1.570000,-2.680000,0.000000,11.200105,0.025456
+N113,-0.648000,0.192000,0.600000,1.570000,-2.680000,0.000000,11.300263,0.105943
+N114,-0.612000,0.258000,0.600000,1.570000,-2.680000,0.000000,11.400077,0.075180
+N115,-0.594000,0.276000,0.600000,1.570000,-2.680000,0.000000,11.500305,0.025456
+N116,-0.552000,0.336000,0.700000,1.570000,-2.680000,0.000000,11.610201,0.123952
+N117,-0.510000,0.384000,0.700000,1.570000,-2.680000,0.000000,11.700544,0.063781
+N118,-0.474000,0.420000,0.700000,1.570000,-2.680000,0.000000,11.800029,0.050912
+N119,-0.444000,0.432000,0.700000,1.570000,-2.680000,0.000000,11.910584,0.032311
+N120,-0.420000,0.444000,0.700000,1.570000,-2.680000,0.000000,12.000417,0.026833
+N121,-0.366000,0.450000,0.700000,1.570000,-2.680000,0.000000,12.099928,0.054332
+N122,-0.306000,0.408000,0.700000,1.570000,-2.680000,0.000000,12.200540,0.073239
+N123,-0.288000,0.294000,0.700000,1.570000,-2.680000,0.000000,12.300222,0.115412
+N124,-0.336000,0.216000,0.700000,1.570000,-2.680000,0.000000,12.400332,0.091586
+N125,-0.348000,0.210000,0.700000,1.570000,-2.680000,0.000000,12.500348,0.013416
+N126,-0.396000,0.258000,0.700000,1.570000,-2.680000,0.000000,12.600793,0.067882
+N127,-0.432000,0.342000,0.700000,1.570000,-2.680000,0.000000,12.709972,0.091389
+N128,-0.438000,0.408000,0.700000,1.570000,-2.680000,0.000000,12.800307,0.066272
+N129,-0.420000,0.456000,0.700000,1.570000,-2.680000,0.000000,12.900536,0.051264
+N130,-0.390000,0.510000,0.700000,1.570000,-2.680000,0.000000,13.000476,0.061774
+N131,-0.342000,0.558000,0.700000,1.570000,-2.680000,0.000000,13.100197,0.067882
+N132,-0.276000,0.624000,0.600000,1.570000,-2.680000,0.000000,13.200265,0.136792
+N133,-0.204000,0.654000,0.600000,1.570000,-2.680000,0.000000,13.300237,0.078000
+N134,-0.090000,0.654000,0.600000,1.570000,-2.680000,0.000000,13.400118,0.114000
+N135,-0.018000,0.582000,0.600000,1.570000,-2.680000,0.000000,13.510477,0.101823
+N136,0.000000,0.504000,0.600000,1.570000,-2.680000,0.000000,13.600797,0.080050
+N137,-0.036000,0.444000,0.600000,1.570000,-2.680000,0.000000,13.710241,0.069971
+N138,-0.078000,0.444000,0.600000,1.570000,-2.680000,0.000000,13.800458,0.042000
+N139,-0.132000,0.492000,0.600000,1.570000,-2.680000,0.000000,13.900422,0.072250
+N140,-0.114000,0.582000,0.600000,1.570000,-2.680000,0.000000,14.000195,0.091782
+N141,-0.084000,0.612000,0.600000,1.570000,-2.680000,0.000000,14.100449,0.042426
+N142,-0.036000,0.612000,0.600000,1.570000,-2.680000,0.000000,14.200224,0.048000
+N143,0.084000,0.612000,0.600000,1.570000,-2.680000,0.000000,14.300595,0.120000
+N144,0.258000,0.600000,0.600000,1.570000,-2.680000,0.000000,14.400253,0.174413
+N145,0.390000,0.576000,0.600000,1.570000,-2.680000,0.000000,14.510297,0.134164
+N146,0.486000,0.498000,0.600000,1.570000,-2.680000,0.000000,14.610196,0.123693
+N147,0.522000,0.390000,0.600000,1.570000,-2.680000,0.000000,14.700868,0.113842
+N148,0.486000,0.276000,0.600000,1.570000,-2.680000,0.000000,14.800189,0.119549
+N149,0.480000,0.168000,0.600000,1.570000,-2.680000,0.000000,14.900086,0.108167
+N150,0.570000,-0.006000,0.600000,1.570000,-2.680000,0.000000,15.010193,0.195898
+N151,0.636000,-0.030000,0.600000,1.570000,-2.680000,0.000000,15.100359,0.070228
+N152,0.726000,0.006000,0.600000,1.570000,-2.680000,0.000000,15.200460,0.096933
+N153,0.756000,0.126000,0.600000,1.570000,-2.680000,0.000000,15.300296,0.123693
+N154,0.690000,0.186000,0.600000,1.570000,-2.680000,0.000000,15.400553,0.089196
+N155,0.624000,0.162000,0.600000,1.570000,-2.680000,0.000000,15.510127,0.070228
+N156,0.594000,0.072000,0.600000,1.570000,-2.680000,0.000000,15.600043,0.094868
+N157,0.594000,-0.012000,0.600000,1.570000,-2.680000,0.000000,15.700522,0.084000
+N158,0.576000,-0.084000,0.600000,1.570000,-2.680000,0.000000,15.800379,0.074216
+N159,0.552000,-0.162000,0.500000,1.570000,-2.680000,0.000000,15.900255,0.129074
+N160,0.516000,-0.216000,0.500000,1.570000,-2.680000,0.000000,16.000173,0.064900
+N161,0.450000,-0.288000,0.500000,1.570000,-2.680000,0.000000,16.100240,0.097673
+N162,0.378000,-0.360000,0.500000,1.570000,-2.680000,0.000000,16.200219,0.101823
+N163,0.300000,-0.414000,0.500000,1.570000,-2.680000,0.000000,16.310500,0.094868
+N164,0.258000,-0.408000,0.500000,1.570000,-2.680000,0.000000,16.410640,0.042426
+N165,0.240000,-0.336000,0.500000,1.570000,-2.680000,0.000000,16.500908,0.074216
+N166,0.282000,-0.252000,0.500000,1.570000,-2.680000,0.000000,16.601070,0.093915
+N167,0.360000,-0.204000,0.500000,1.570000,-2.680000,0.000000,16.700032,0.091586
+N168,0.432000,-0.168000,0.500000,1.570000,-2.680000,0.000000,16.800286,0.080498
+N169,0.510000,-0.102000,0.500000,1.570000,-2.680000,0.000000,16.900659,0.102176
+N170,0.552000,-0.018000,0.500000,1.570000,-2.680000,0.000000,17.010185,0.093915
+N171,0.534000,0.108000,0.400000,1.570000,-2.680000,0.000000,17.110313,0.161864
+N172,0.384000,0.264000,0.400000,1.570000,-2.680000,0.000000,17.210100,0.216416
+N173,0.264000,0.396000,0.400000,1.570000,-2.680000,0.000000,17.310141,0.178393
+N174,0.252000,0.438000,0.400000,1.570000,-2.680000,0.000000,17.400254,0.043681
+N175,0.324000,0.486000,0.400000,1.570000,-2.680000,0.000000,17.500255,0.086533
+N176,0.342000,0.486000,0.400000,1.570000,-2.680000,0.000000,17.600364,0.018000
+N177,0.390000,0.456000,0.400000,1.570000,-2.680000,0.000000,17.700455,0.056604
+N178,0.438000,0.348000,0.400000,1.570000,-2.680000,0.000000,17.800207,0.118186
+N179,0.474000,0.270000,0.400000,1.570000,-2.680000,0.000000,17.900158,0.085907
+N180,0.492000,0.192000,0.400000,1.570000,-2.680000,0.000000,18.010174,0.080050
+N181,0.504000,0.114000,0.400000,1.570000,-2.680000,0.000000,18.110538,0.078918
+N182,0.612000,0.090000,0.400000,1.570000,-2.680000,0.000000,18.210238,0.110635
+N183,0.696000,0.138000,0.400000,1.570000,-2.680000,0.000000,18.300022,0.096747
+N184,0.588000,0.330000,0.400000,1.570000,-2.680000,0.000000,18.400343,0.220291
+N185,0.540000,0.354000,0.400000,1.570000,-2.680000,0.000000,18.500013,0.053666

src/roboglue_ros/config/data/test12.data

@@ -0,0 +1,99 @@
+N,X,Y,Z,rR,rP,rY,DT,DS
+N0,-0.240000,0.726000,0.200000,1.570000,-2.680000,0.000000,0.000000,0.000000
+N1,-0.012000,0.498000,0.200000,1.570000,-2.680000,0.000000,1.929977,0.322441
+N2,-0.012000,0.480000,0.200000,1.570000,-2.680000,0.000000,2.030104,0.018000
+N3,-0.012000,0.474000,0.200000,1.570000,-2.680000,0.000000,2.129790,0.006000
+N4,-0.012000,0.468000,0.200000,1.570000,-2.680000,0.000000,2.230435,0.006000
+N5,-0.006000,0.462000,0.200000,1.570000,-2.680000,0.000000,2.329622,0.008485
+N6,0.006000,0.450000,0.200000,1.570000,-2.680000,0.000000,2.429942,0.016971
+N7,0.024000,0.450000,0.200000,1.570000,-2.680000,0.000000,2.530050,0.018000
+N8,0.042000,0.438000,0.200000,1.570000,-2.680000,0.000000,2.629587,0.021633
+N9,0.060000,0.438000,0.200000,1.570000,-2.680000,0.000000,2.730013,0.018000
+N10,0.072000,0.438000,0.200000,1.570000,-2.680000,0.000000,2.829691,0.012000
+N11,0.078000,0.438000,0.200000,1.570000,-2.680000,0.000000,2.930148,0.006000
+N12,0.096000,0.438000,0.300000,1.570000,-2.680000,0.000000,3.029669,0.101607
+N13,0.132000,0.420000,0.300000,1.570000,-2.680000,0.000000,3.129793,0.040249
+N14,0.150000,0.414000,0.300000,1.570000,-2.680000,0.000000,3.230084,0.018974
+N15,0.174000,0.408000,0.300000,1.570000,-2.680000,0.000000,3.320733,0.024739
+N16,0.204000,0.396000,0.300000,1.570000,-2.680000,0.000000,3.429635,0.032311
+N17,0.258000,0.384000,0.300000,1.570000,-2.680000,0.000000,3.529731,0.055317
+N18,0.336000,0.378000,0.300000,1.570000,-2.680000,0.000000,3.630354,0.078230
+N19,0.396000,0.372000,0.300000,1.570000,-2.680000,0.000000,3.729873,0.060299
+N20,0.438000,0.372000,0.400000,1.570000,-2.680000,0.000000,3.830100,0.108462
+N21,0.468000,0.366000,0.400000,1.570000,-2.680000,0.000000,3.930035,0.030594
+N22,0.510000,0.366000,0.400000,1.570000,-2.680000,0.000000,4.029916,0.042000
+N23,0.528000,0.378000,0.400000,1.570000,-2.680000,0.000000,4.129753,0.021633
+N24,0.558000,0.426000,0.400000,1.570000,-2.680000,0.000000,4.230277,0.056604
+N25,0.558000,0.438000,0.400000,1.570000,-2.680000,0.000000,4.330536,0.012000
+N26,0.564000,0.456000,0.400000,1.570000,-2.680000,0.000000,4.429635,0.018974
+N27,0.564000,0.474000,0.400000,1.570000,-2.680000,0.000000,4.519780,0.018000
+N28,0.564000,0.516000,0.400000,1.570000,-2.680000,0.000000,4.629593,0.042000
+N29,0.558000,0.534000,0.500000,1.570000,-2.680000,0.000000,4.729752,0.101784
+N30,0.546000,0.552000,0.500000,1.570000,-2.680000,0.000000,4.819632,0.021633
+N31,0.522000,0.582000,0.500000,1.570000,-2.680000,0.000000,4.929755,0.038419
+N32,0.474000,0.624000,0.500000,1.570000,-2.680000,0.000000,5.129712,0.063781
+N33,0.438000,0.642000,0.500000,1.570000,-2.680000,0.000000,5.230273,0.040249
+N34,0.402000,0.666000,0.500000,1.570000,-2.680000,0.000000,5.329853,0.043267
+N35,0.366000,0.678000,0.500000,1.570000,-2.680000,0.000000,5.429908,0.037947
+N36,0.324000,0.708000,0.500000,1.570000,-2.680000,0.000000,5.530349,0.051614
+N37,0.306000,0.714000,0.500000,1.570000,-2.680000,0.000000,5.629743,0.018974
+N38,0.288000,0.714000,0.500000,1.570000,-2.680000,0.000000,5.729661,0.018000
+N39,0.258000,0.714000,0.400000,1.570000,-2.680000,0.000000,5.829637,0.104403
+N40,0.222000,0.714000,0.400000,1.570000,-2.680000,0.000000,5.929677,0.036000
+N41,0.186000,0.714000,0.400000,1.570000,-2.680000,0.000000,6.029933,0.036000
+N42,0.150000,0.714000,0.400000,1.570000,-2.680000,0.000000,6.129757,0.036000
+N43,0.120000,0.714000,0.400000,1.570000,-2.680000,0.000000,6.230042,0.030000
+N44,0.078000,0.714000,0.400000,1.570000,-2.680000,0.000000,6.329963,0.042000
+N45,0.048000,0.708000,0.500000,1.570000,-2.680000,0.000000,6.429848,0.104575
+N46,0.024000,0.708000,0.500000,1.570000,-2.680000,0.000000,6.530149,0.024000
+N47,-0.006000,0.708000,0.500000,1.570000,-2.680000,0.000000,6.630249,0.030000
+N48,-0.042000,0.708000,0.500000,1.570000,-2.680000,0.000000,6.730454,0.036000
+N49,-0.078000,0.708000,0.500000,1.570000,-2.680000,0.000000,6.829878,0.036000
+N50,-0.114000,0.708000,0.500000,1.570000,-2.680000,0.000000,6.929832,0.036000
+N51,-0.144000,0.708000,0.600000,1.570000,-2.680000,0.000000,7.029889,0.104403
+N52,-0.186000,0.708000,0.600000,1.570000,-2.680000,0.000000,7.129906,0.042000
+N53,-0.222000,0.702000,0.600000,1.570000,-2.680000,0.000000,7.229855,0.036497
+N54,-0.300000,0.666000,0.600000,1.570000,-2.680000,0.000000,7.329896,0.085907
+N55,-0.366000,0.642000,0.600000,1.570000,-2.680000,0.000000,7.429569,0.070228
+N56,-0.390000,0.630000,0.600000,1.570000,-2.680000,0.000000,7.530061,0.026833
+N57,-0.432000,0.606000,0.700000,1.570000,-2.680000,0.000000,7.630038,0.111086
+N58,-0.462000,0.588000,0.700000,1.570000,-2.680000,0.000000,7.730011,0.034986
+N59,-0.474000,0.570000,0.700000,1.570000,-2.680000,0.000000,7.829794,0.021633
+N60,-0.492000,0.540000,0.700000,1.570000,-2.680000,0.000000,7.929635,0.034986
+N61,-0.504000,0.522000,0.700000,1.570000,-2.680000,0.000000,8.029778,0.021633
+N62,-0.516000,0.492000,0.700000,1.570000,-2.680000,0.000000,8.129793,0.032311
+N63,-0.528000,0.462000,0.700000,1.570000,-2.680000,0.000000,8.230089,0.032311
+N64,-0.528000,0.438000,0.800000,1.570000,-2.680000,0.000000,8.330544,0.102840
+N65,-0.522000,0.420000,0.800000,1.570000,-2.680000,0.000000,8.430031,0.018974
+N66,-0.486000,0.390000,0.800000,1.570000,-2.680000,0.000000,8.530712,0.046861
+N67,-0.426000,0.378000,0.800000,1.570000,-2.680000,0.000000,8.629827,0.061188
+N68,-0.354000,0.378000,0.800000,1.570000,-2.680000,0.000000,8.729755,0.072000
+N69,-0.294000,0.390000,0.800000,1.570000,-2.680000,0.000000,8.829925,0.061188
+N70,-0.228000,0.414000,0.800000,1.570000,-2.680000,0.000000,8.929969,0.070228
+N71,-0.126000,0.456000,0.700000,1.570000,-2.680000,0.000000,9.030209,0.148889
+N72,-0.024000,0.492000,0.700000,1.570000,-2.680000,0.000000,9.129969,0.108167
+N73,0.024000,0.510000,0.700000,1.570000,-2.680000,0.000000,9.230358,0.051264
+N74,0.096000,0.534000,0.700000,1.570000,-2.680000,0.000000,9.329993,0.075895
+N75,0.120000,0.534000,0.700000,1.570000,-2.680000,0.000000,9.430514,0.024000
+N76,0.162000,0.540000,0.700000,1.570000,-2.680000,0.000000,9.530359,0.042426
+N77,0.228000,0.546000,0.600000,1.570000,-2.680000,0.000000,9.630286,0.119967
+N78,0.300000,0.516000,0.600000,1.570000,-2.680000,0.000000,9.730100,0.078000
+N79,0.336000,0.486000,0.600000,1.570000,-2.680000,0.000000,9.830563,0.046861
+N80,0.354000,0.456000,0.600000,1.570000,-2.680000,0.000000,9.919751,0.034986
+N81,0.372000,0.432000,0.600000,1.570000,-2.680000,0.000000,10.029628,0.030000
+N82,0.390000,0.402000,0.600000,1.570000,-2.680000,0.000000,10.129580,0.034986
+N83,0.414000,0.384000,0.600000,1.570000,-2.680000,0.000000,10.229966,0.030000
+N84,0.444000,0.360000,0.500000,1.570000,-2.680000,0.000000,10.329809,0.107126
+N85,0.486000,0.342000,0.500000,1.570000,-2.680000,0.000000,10.429798,0.045695
+N86,0.522000,0.342000,0.500000,1.570000,-2.680000,0.000000,10.529751,0.036000
+N87,0.552000,0.342000,0.500000,1.570000,-2.680000,0.000000,10.629892,0.030000
+N88,0.564000,0.360000,0.500000,1.570000,-2.680000,0.000000,10.719820,0.021633
+N89,0.576000,0.372000,0.500000,1.570000,-2.680000,0.000000,10.829559,0.016971
+N90,0.576000,0.384000,0.500000,1.570000,-2.680000,0.000000,10.929559,0.012000
+N91,0.558000,0.408000,0.400000,1.570000,-2.680000,0.000000,11.030504,0.104403
+N92,0.516000,0.444000,0.400000,1.570000,-2.680000,0.000000,11.130024,0.055317
+N93,0.480000,0.468000,0.400000,1.570000,-2.680000,0.000000,11.229671,0.043267
+N94,0.438000,0.486000,0.400000,1.570000,-2.680000,0.000000,11.329660,0.045695
+N95,0.408000,0.498000,0.400000,1.570000,-2.680000,0.000000,11.429784,0.032311
+N96,0.384000,0.510000,0.400000,1.570000,-2.680000,0.000000,11.529714,0.026833
+N97,0.384000,0.516000,0.400000,1.570000,-2.680000,0.000000,11.629770,0.006000

src/roboglue_ros/config/meta/.meta

@@ -0,0 +1 @@
+{"code":"{abc23611-98d6-4032-b3a8-41f6e750b725}","ds":0,"dt":0,"frame_base":"---","frame_work":{"name":"---","offset":{"r":0.0,"x":0.0,"y":0.0,"z":0.0}},"n_aux":1,"n_point":35,"name":"","type":"time/dist"}

src/roboglue_ros/config/meta/@meta_template.meta

@@ -0,0 +1,20 @@
+{
+	"name": "---",
+	"code": "---",
+	"frame_base": "---",
+	"frame_work": {
+		"name": "---",
+		"offset": {
+			"x": 0.0,
+			"y": 0.0,
+			"z": 0.0,
+			"r": 0.0
+		}
+	},
+	"type": "time/dist",
+	"dt": 0,
+	"ds": 0,
+	"n_point": 0,
+	"n_digital": 0,
+	"n_analog": 0
+}

src/roboglue_ros/config/meta/aaaa.meta

@@ -0,0 +1 @@
+{"code":"{93f6e2fc-ec18-4ca7-b606-ca0678a9f456}","ds":3.0,"dt":10.0,"frame_base":"---","frame_work":{"name":"---","offset":{"r":0.0,"x":0.0,"y":0.0,"z":0.0}},"n_analog":0,"n_digital":0,"n_point":53,"name":"aaaa","type":"time/dist"}

src/roboglue_ros/config/meta/abcd.meta

@@ -0,0 +1 @@
+{"code":"{54ed0e5e-7506-4fce-a8f6-0782efcd94de}","ds":10.5,"dt":0.5,"frame_base":"---","frame_work":{"name":"---","offset":{"r":0.0,"x":0.0,"y":0.0,"z":0.0}},"n_analog":0,"n_digital":0,"n_point":68,"name":"abcd","type":"time/dist"}

src/roboglue_ros/config/meta/test01.meta

@@ -0,0 +1 @@
+{"code":"{f32f3c89-50d3-4c2e-b090-3e1fe66530bd}","ds":100.0,"dt":1.0,"frame_base":"---","frame_work":{"name":"---","offset":{"r":0.0,"x":0.0,"y":0.0,"z":0.0}},"n_analog":0,"n_digital":0,"n_point":213,"name":"test01","type":"time/dist"}

src/roboglue_ros/config/meta/test02.meta

@@ -0,0 +1 @@
+{"code":"{ec4f7968-ed79-4b9f-af94-a4f36e0cf998}","ds":1.0,"dt":0.5,"frame_base":"---","frame_work":{"name":"---","offset":{"r":0.0,"x":0.0,"y":0.0,"z":0.0}},"n_analog":0,"n_digital":0,"n_point":42,"name":"test02","type":"time/dist"}

src/roboglue_ros/config/meta/test03.meta

@@ -0,0 +1 @@
+{"code":"{880baa86-0179-4923-9145-5455a8516585}","ds":1.0,"dt":0.5,"frame_base":"---","frame_work":{"name":"---","offset":{"r":0.0,"x":0.0,"y":0.0,"z":0.0}},"n_analog":0,"n_digital":0,"n_point":36,"name":"test03","type":"time/dist"}

src/roboglue_ros/config/meta/test04.meta

@@ -0,0 +1 @@
+{"code":"{146cd418-d343-4750-9f5c-2533d3b86d8f}","ds":100.0,"dt":0.5,"frame_base":"---","frame_work":{"name":"---","offset":{"r":0.0,"x":0.0,"y":0.0,"z":0.0}},"n_analog":0,"n_digital":0,"n_point":116,"name":"test04","type":"time/dist"}

src/roboglue_ros/config/meta/test05.meta

@@ -0,0 +1 @@
+{"code":"{2f11c915-9248-4fae-ac06-1e4edfa73081}","ds":100.0,"dt":0.5,"frame_base":"---","frame_work":{"name":"---","offset":{"r":0.0,"x":0.0,"y":0.0,"z":0.0}},"n_analog":0,"n_digital":0,"n_point":48,"name":"test05","type":"time/dist"}

src/roboglue_ros/config/meta/test06.meta

@@ -0,0 +1 @@
+{"code":"{6aefb72e-1376-417e-9e07-b91b9943d304}","ds":100.0,"dt":0.5,"frame_base":"---","frame_work":{"name":"---","offset":{"r":0.0,"x":0.0,"y":0.0,"z":0.0}},"n_analog":0,"n_digital":0,"n_point":43,"name":"test06","type":"time/dist"}

src/roboglue_ros/config/meta/test07.meta

@@ -0,0 +1 @@
+{"code":"{a7e5a537-e7e4-45b2-a3db-8bc998784d02}","ds":20.0,"dt":0.5,"frame_base":"---","frame_work":{"name":"---","offset":{"r":0.0,"x":0.0,"y":0.0,"z":0.0}},"n_analog":0,"n_digital":0,"n_point":98,"name":"test07","type":"time/dist"}

src/roboglue_ros/config/meta/test08.meta

@@ -0,0 +1 @@
+{"code":"{2a694d2f-f93c-4822-b0c3-9520886e3f48}","ds":100.0,"dt":0.5,"frame_base":"---","frame_work":{"name":"---","offset":{"r":0.0,"x":0.0,"y":0.0,"z":0.0}},"n_analog":0,"n_digital":0,"n_point":104,"name":"test08","type":"time/dist"}

src/roboglue_ros/config/meta/test09.meta

@@ -0,0 +1 @@
+{"code":"{846e5f31-1384-437f-bfd3-e7c49b8adc9a}","ds":100.0,"dt":0.5,"frame_base":"---","frame_work":{"name":"---","offset":{"r":0.0,"x":0.0,"y":0.0,"z":0.0}},"n_analog":0,"n_digital":0,"n_point":270,"name":"test09","type":"time/dist"}

src/roboglue_ros/config/meta/test10.meta

@@ -0,0 +1 @@
+{"code":"{7d063eec-2815-4e20-8907-41505c4f92a3}","ds":100.0,"dt":0.5,"frame_base":"---","frame_work":{"name":"---","offset":{"r":0.0,"x":0.0,"y":0.0,"z":0.0}},"n_analog":0,"n_digital":0,"n_point":280,"name":"test10","type":"time/dist"}

src/roboglue_ros/config/meta/test11.meta

@@ -0,0 +1 @@
+{"code":"{34c60b4e-5a83-4608-b321-8ec77e2a9d11}","ds":100.0,"dt":0.5,"frame_base":"---","frame_work":{"name":"---","offset":{"r":0.0,"x":0.0,"y":0.0,"z":0.0}},"n_analog":0,"n_digital":0,"n_point":186,"name":"test11","type":"time/dist"}

src/roboglue_ros/config/meta/test12.meta

@@ -0,0 +1 @@
+{"code":"{585ee04d-1940-424d-8d07-8878ba148f03}","ds":100.0,"dt":0.5,"frame_base":"---","frame_work":{"name":"---","offset":{"r":0.0,"x":0.0,"y":0.0,"z":0.0}},"n_analog":0,"n_digital":0,"n_point":98,"name":"test12","type":"time/dist"}

src/roboglue_ros/include/roboglue_ros/roboglue_utils.h

@@ -0,0 +1,114 @@
+#ifndef ROBOGLUE_UTILS_H
+#define ROBOGLUE_UTILS_H
+
+#endif // ROBOGLUE_UTILS_H
+
+#include <iostream>
+#include <stdlib.h>
+#include <ros/ros.h>
+#include <boost/bind.hpp>
+#include <boost/algorithm/string.hpp>
+#include <std_msgs/String.h>
+#include <eigen_conversions/eigen_msg.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/TwistStamped.h>
+#include <trajectory_msgs/JointTrajectory.h>
+
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_state/robot_state.h>
+#include <moveit_visual_tools/moveit_visual_tools.h>
+
+#include <Eigen/Eigen>
+#include <tf/tf.h>
+
+#include <roboglue_ros/json.hpp>
+#include <roboglue_ros/rapidcsv.h>
+
+typedef std::map<std::string, ros::Publisher*> publisherMap;
+
+/// setup rviz palnned path visualization
+namespace rvt=rviz_visual_tools;
+namespace mvt=moveit_visual_tools;
+
+///////// MQTT & ROS TOPIC NAMES ///////////////
+typedef struct {
+    std::string MQTTcommandPub, MQTTcoordPub, MQTTstatePub;
+    std::string MQTTcommandSub, MQTTcoordSub, MQTTstateSub;
+    std::string mqttHost;
+    int mqttQoS;
+} MQTT_topics;
+
+typedef struct {
+    std::string commandPub, coordPub, statePub;
+    std::string commandSub, coordSub, stateSub;
+    std::string delta_jog, delta_joint_jog, traj_jog;
+} ROS_topics;
+
+///////// AUX DATA STORAGE ///////////////
+typedef struct {
+    std::list<std::pair<std::string,bool>> digital;
+    std::list<std::pair<std::string,uint16_t>> analog;
+} auxData;
+
+////////// PLAY & RECORD DATA SORAGE ////////
+typedef struct {
+    geometry_msgs::TwistStamped point;
+    size_t pointNumber;
+    double dS;
+    auxData pointAux;
+} pointRecord;
+
+////////// INTERNAL STATE STORAGE //////////
+typedef struct{
+    bool isFileOpen = false;
+    bool isPlaying = false;
+    bool isRecording = false;
+    bool isRealtime = false;
+    bool isRunning = true;
+} internalState;
+
+////////// POSITION DATA STORAGE ///////////
+typedef struct {
+    geometry_msgs::TwistStamped twist;
+    trajectory_msgs::JointTrajectory joint;
+    bool waitIK = false;
+    bool isFirst = false;
+    bool isNew = false;
+    bool isJoint = false;
+    bool isTwist = false;
+    bool isVel = false;
+    bool isPos = false;
+    bool lockX = false;
+    bool lockY = false;
+    bool lockZ = false;
+    bool lockRX = false;
+    bool lockRY = false;
+    bool lockRZ = false;
+} positionData;
+
+///////// ROBOT DATA STORAGE /////////////
+typedef struct {
+    std::string robot_model_name, move_group_name, base_frame;
+    std::string point_group_mode, planning_mode;
+    const robot_state::JointModelGroup* joint_modelGroup;
+    robot_model::RobotModelConstPtr kine_model;
+    robot_state::RobotStatePtr kine_state;
+    std::vector<std::string> joint_names;
+} robotData;
+
+typedef struct {
+    mvt::MoveItVisualToolsPtr vtools;
+} rvizData;
+
+geometry_msgs::TwistStamped jstr2Twist(nlohmann::json);
+trajectory_msgs::JointTrajectory jstr2Joint(nlohmann::json);
+
+void getLockInfo(positionData*, nlohmann::json);
+geometry_msgs::TwistStamped lockTwistPose(geometry_msgs::TwistStamped, geometry_msgs::TwistStamped, positionData);
+geometry_msgs::PoseStamped twist2Pose(geometry_msgs::TwistStamped);
+double e3dist(geometry_msgs::Twist, geometry_msgs::Twist);
+double rad2deg(double ang);
+bool checkJointJump(moveit_msgs::RobotTrajectoryPtr, double);

src/roboglue_ros/launch/roboglue_ros.launch

@@ -0,0 +1,70 @@
+<launch>
+    <!-- Set logger format -->
+    <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${node}]: ${message}" />
+    <!--    <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${node}] [${function}]: ${message}" />-->
+
+    <!-- Start ROS controller and moveit API interface-->
+    <include file="$(find ur_modern_driver)/launch/ur10e_ros_control.launch">
+        <arg name="robot_ip" value="10.0.0.5" />
+    </include>
+    <include file="$(find ur10_e_moveit_config)/launch/ur10_e_moveit_planning_execution.launch">
+        <!-- <arg name="robot_ip" value="10.0.0.5" /> -->
+    </include>
+    <include file="$(find ur10_e_moveit_config)/launch/moveit_rviz.launch">
+        <arg name="config" value="true" />
+    </include>
+
+    <!-- Common parameter Server -->
+    <group ns="roboglue_ros_common">
+        <param name="INstate"   type="string" value="/roboglue_com/com2ros/state" />
+        <param name="INcommands" type="string" value="/roboglue_com/com2ros/commands" />
+        <param name="INcoordinates" type="string" value="/roboglue_com/com2ros/coordinates" />
+        <param name="OUTstate" type="string" value="/roboglue_com/ros2com/state" />
+        <param name="OUTcommands" type="string" value="/roboglue_com/ros2com/commands" />
+        <param name="OUTcoordinates" type="string" value="/roboglue_com/ros2com/coordinates" />
+        <param name="OUTdelta_jog" type="string" value="/jog_arm_server/delta_jog_cmds" />
+        <param name="OUTdelta_joint_jog" type="string" value="/jog_arm_server/delta_joint_jog_cmds" />
+        <param name="OUTtraj_jog" type="string" value="/pos_based_pos_traj_controller/command" />
+        <param name="robot_model_name" type="string" value="robot_description" />
+        <param name="move_group_name" type="string" value="manipulator" />
+        <param name="loop_rate" type="int" value="100" />
+        <param name="msg_buffer" type="int" value="100" />
+        <param name="min_nonzero_move" type="double" value="0.005" />
+    </group>
+
+    <!-- Message Relay to/from MQTT -->
+    <node name="roboglue_ros_relay" pkg="roboglue_ros" type="roboglue_ros_relay" output="screen" required="true">
+        <param name="parameter_ns" type="string" value="/roboglue_ros_common/" />
+        <param name="mqtt_host" type="string" value="tcp://localhost:1883" />
+        <param name="mqtt_qos" type="int" value="0" />
+        <param name="mqtt_INstate" type="string" value="roboglue_com/com2ros/state" />
+        <param name="mqtt_INcommands" type="string" value="roboglue_com/com2ros/commands" />
+        <param name="mqtt_INcoordinates" type="string" value="roboglue_com/com2ros/coordinates" />
+        <param name="mqtt_OUTstate" type="string" value="roboglue_com/ros2com/state" />
+        <param name="mqtt_OUTcommands" type="string" value="roboglue_com/ros2com/commands" />
+        <param name="mqtt_OUTcoordinates" type="string" value="roboglue_com/ros2com/coordinates" />
+    </node>
+
+    <!-- Real Time Tracking -->
+    <node name="roboglue_ros_follower" pkg="roboglue_ros" type="roboglue_ros_follower" output="screen" required="true">
+        <param name="parameter_ns" type="string" value="roboglue_ros_common" />
+        <param name="jog_pub_rate" type="int" value="5" />
+    </node>
+
+    <!-- Tracking data Recorder/Player -->
+    <node name="roboglue_ros_recorder" pkg="roboglue_ros" type="roboglue_ros_recorder" output="screen" required="true">
+        <param name="parameter_ns" type="string" value="/roboglue_ros_common/" />
+        <param name="point_group_mode" type="string" value="dist" />
+        <param name="planning_mode" type="string" value="path" />
+        <param name="meta_dir" type="string" value="/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src/roboglue_ros/config/meta/" />
+        <param name="data_dir" type="string" value="/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src/roboglue_ros/config/data/" />
+        <param name="meta_ext" type="string" value=".meta" />
+        <param name="data_ext" type="string" value=".data" />
+        <param name="meta_template" type="string" value="@meta_template.meta" />
+        <param name="data_template" type="string" value="@data_template.data" />
+    </node>
+
+    <!-- <param name="planning_mode" type="string" value="joint" />   -->
+    <!-- <param name="point_group_mode" type="string" value="time" /> -->
+
+</launch>

src/roboglue_ros/launch/roboglue_ros_urdriver.launch

@@ -0,0 +1,70 @@
+<launch>
+    <!-- Set logger format -->
+    <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${node}]: ${message}" />
+    <!--    <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${node}] [${function}]: ${message}" />-->
+
+    <!-- Start ROS controller and moveit API interface-->
+    <include file="$(find ur_modern_driver)/launch/ur10e_ros_control.launch">
+        <arg name="robot_ip" value="10.0.0.5" />
+    </include>
+    <include file="$(find ur10_e_moveit_config)/launch/ur10_e_moveit_planning_execution.launch">
+        <!-- <arg name="robot_ip" value="10.0.0.5" /> -->
+    </include>
+    <include file="$(find ur10_e_moveit_config)/launch/moveit_rviz.launch">
+        <arg name="config" value="true" />
+    </include>
+
+    <!-- Common parameter Server -->
+    <group ns="roboglue_ros_common">
+        <param name="INstate"   type="string" value="/roboglue_com/com2ros/state" />
+        <param name="INcommands" type="string" value="/roboglue_com/com2ros/commands" />
+        <param name="INcoordinates" type="string" value="/roboglue_com/com2ros/coordinates" />
+        <param name="OUTstate" type="string" value="/roboglue_com/ros2com/state" />
+        <param name="OUTcommands" type="string" value="/roboglue_com/ros2com/commands" />
+        <param name="OUTcoordinates" type="string" value="/roboglue_com/ros2com/coordinates" />
+        <param name="OUTdelta_jog" type="string" value="/jog_arm_server/delta_jog_cmds" />
+        <param name="OUTdelta_joint_jog" type="string" value="/jog_arm_server/delta_joint_jog_cmds" />
+        <param name="OUTtraj_jog" type="string" value="/pos_based_pos_traj_controller/command" />
+        <param name="robot_model_name" type="string" value="robot_description" />
+        <param name="move_group_name" type="string" value="manipulator" />
+        <param name="loop_rate" type="int" value="100" />
+        <param name="msg_buffer" type="int" value="100" />
+        <param name="min_nonzero_move" type="double" value="0.005" />
+    </group>
+
+    <!-- Message Relay to/from MQTT -->
+    <node name="roboglue_ros_relay" pkg="roboglue_ros" type="roboglue_ros_relay" output="screen" required="true">
+        <param name="parameter_ns" type="string" value="/roboglue_ros_common/" />
+        <param name="mqtt_host" type="string" value="tcp://localhost:1883" />
+        <param name="mqtt_qos" type="int" value="0" />
+        <param name="mqtt_INstate" type="string" value="roboglue_com/com2ros/state" />
+        <param name="mqtt_INcommands" type="string" value="roboglue_com/com2ros/commands" />
+        <param name="mqtt_INcoordinates" type="string" value="roboglue_com/com2ros/coordinates" />
+        <param name="mqtt_OUTstate" type="string" value="roboglue_com/ros2com/state" />
+        <param name="mqtt_OUTcommands" type="string" value="roboglue_com/ros2com/commands" />
+        <param name="mqtt_OUTcoordinates" type="string" value="roboglue_com/ros2com/coordinates" />
+    </node>
+
+    <!-- Real Time Tracking -->
+    <node name="roboglue_ros_follower" pkg="roboglue_ros" type="roboglue_ros_follower" output="screen" required="true">
+        <param name="parameter_ns" type="string" value="roboglue_ros_common" />
+        <param name="jog_pub_rate" type="int" value="5" />
+    </node>
+
+    <!-- Tracking data Recorder/Player -->
+    <node name="roboglue_ros_recorder" pkg="roboglue_ros" type="roboglue_ros_recorder" output="screen" required="true">
+        <param name="parameter_ns" type="string" value="/roboglue_ros_common/" />
+        <param name="point_group_mode" type="string" value="dist" />
+        <param name="planning_mode" type="string" value="path" />
+        <param name="meta_dir" type="string" value="/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src/roboglue_ros/config/meta/" />
+        <param name="data_dir" type="string" value="/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src/roboglue_ros/config/data/" />
+        <param name="meta_ext" type="string" value=".meta" />
+        <param name="data_ext" type="string" value=".data" />
+        <param name="meta_template" type="string" value="@meta_template.meta" />
+        <param name="data_template" type="string" value="@data_template.data" />
+    </node>
+
+    <!-- <param name="planning_mode" type="string" value="joint" />   -->
+    <!-- <param name="point_group_mode" type="string" value="time" /> -->
+
+</launch>

src/roboglue_ros/package.xml

@@ -0,0 +1,63 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>roboglue_ros</name>
+  <version>0.0.2</version>
+  <description>The roboglue_ros package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="emanuele@souplesse.it">Emanuele</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>GPLv3</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/roboglue_ros</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+  <author >Emanuele</author>
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/roboglue_ros/src/roboglue_follower.cpp

@@ -0,0 +1,278 @@
+/*
+20190220 - Inizia la scrittura del nodo che si occupa di replicare i movimenti realtime
+20190305 - Primi test sul nodo, uso della funzione di calcolo cinematica inversa senza limiti o
+            preferenze per lo spazio dei giunti.
+            Il movimento è abbastanza imprevedibile e tende a cambiare configurazione quando
+            si oltrepassano gli assi X e Y. Un rateo di 5Hz è abbastanza per ottenere un movimento
+            quasi fluido del braccio. Per capire meglio serve un braccio reale $$$
+            Modificato il file kinematics.yaml per usare il solver incluso nel driver
+20190308 - Inserita una funzione che si occupa di effettuare il blocco delle variaili.
+20190325 - Implementato uso del ros parameter server per la configurazione del nodo.
+            Contiene i nomi delle code ros.
+*/
+////////// ROS INCLUDES /////////////
+#include "ros/ros.h"
+#include <roboglue_ros/roboglue_utils.h> /// utility function library
+#include <moveit/move_group_interface/move_group_interface.h>
+
+
+////////////////////////////////////////////
+///////// FUNCTION DECLARATIONS ///////////
+//////////////////////////////////////////
+void testFunction( ros::Publisher* trj_pub);
+trajectory_msgs::JointTrajectory composeTrjMessage(std::vector<double> jtpos);
+
+///////////////////////////////////////////
+///////// ROS CALLBACKS //////////////////
+/////////////////////////////////////////
+void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, positionData* pos) {
+    ROS_DEBUG("Received command: [%s]", msg->data.c_str());
+    try {
+        nlohmann::json c = nlohmann::json::parse(msg->data.c_str());
+        std::string command = c["command"];
+        nlohmann::json params = c["params"];
+        if (!command.compare("QUIT")){
+            is->isRunning = false;
+        } else if (!command.compare("REALTIME")){
+            if (!params["action"].get<std::string>().compare("start")){
+                is->isRealtime=true;
+                is->isPlaying=false;
+                is->isRecording=false;
+                getLockInfo(pos, params["lock"]);
+            }
+            else if(!params["action"].get<std::string>().compare("stop")) {
+                is->isRealtime = false;
+            }
+            else ROS_ERROR("Invalid acton");
+        } else if (!command.compare("RECORD") || !command.compare("PLAY") || !command.compare("OPEN")) {
+            return;
+        } else {
+            ROS_WARN("Unknown command: %s", command.c_str());
+        }
+    } catch (nlohmann::json::exception){
+        ROS_ERROR("Failed to parse command!");
+    }
+}
+
+void coordinateCallback(const std_msgs::String::ConstPtr& msg, publisherMap* pubs, positionData* pos){
+    ROS_DEBUG("Received coordinate: [%s]", msg->data.c_str());
+    nlohmann::json coordinateParsed = nlohmann::json::parse(msg->data);
+    std::string type = coordinateParsed["type"];
+    std::string mode = coordinateParsed["mode"];
+    geometry_msgs::TwistStamped twPose;
+    trajectory_msgs::JointTrajectory jtPose;
+
+    if (!pos->waitIK){
+        if (!mode.compare("pos")){
+            pos->isPos=true;
+            pos->isVel=false;
+        } else if (!mode.compare("vel")){
+            pos->isPos=false;
+            pos->isVel=true;
+        }
+        if (!type.compare("cartesian")){
+            pos->isTwist = true;
+            pos->isJoint = false;
+            if(!mode.compare("pos")){
+                twPose = jstr2Twist(coordinateParsed["value"]); // get desired stamped pose from message
+                pos->twist = twPose;
+            } else if (!mode.compare("vel")) { // Publish to jog_arm_server
+                // TODO implementare
+            }
+        } else if (!type.compare("joint")){
+            pos->isTwist = false;
+            pos->isJoint = true;
+            if(!mode.compare("pos")){
+                jtPose = jstr2Joint(coordinateParsed["value"]);
+                pos->joint = jtPose;
+            } else if (!mode.compare("vel")){
+                // TODO implementare
+            }
+        } else {
+            ROS_ERROR("Unknown coordinate type");
+        }
+    }
+}
+
+void stateCallback(const std_msgs::String::ConstPtr& msg, internalState* is, auxData* aux){
+    ROS_DEBUG("Received aux I/O State Update: [%s]", msg->data.c_str());
+}
+
+void timedPublishCallback(const ros::TimerEvent&, internalState* is, publisherMap* pubs, positionData* pos, robotData* rdata){
+    static geometry_msgs::TwistStamped lasttwPose;
+    static trajectory_msgs::JointTrajectory lastjtPose;
+    static bool firstIK = true;
+    geometry_msgs::TwistStamped twPose;
+    trajectory_msgs::JointTrajectory jtPose;
+    geometry_msgs::PoseStamped newPose;
+    double timeout = 0.1;
+    uint attempts = 10;
+    bool foundIK = false;
+    std::vector<double> jPos, jStartPos = {M_PI/2, -3*M_PI/4, +3*M_PI/4, -M_PI/2, -M_PI/2, 0};
+
+    if (is->isRealtime){
+        twPose = pos->twist;
+        jtPose = pos->joint;
+        pos->waitIK = true;
+        if (pos->isTwist){
+            twPose = lockTwistPose(twPose, lasttwPose, *pos);
+            lasttwPose = twPose;
+            newPose = twist2Pose(twPose);
+            ROS_DEBUG_COND(false,"Finding IK for pose: X:%6.3f, Y:%6.3f, Z:%6.3f \n\
+                                            qX:%6.3f, qY:%6.3f, qZ:%6.3f, w:%6.3f",
+                      newPose.pose.position.x, newPose.pose.position.y, newPose.pose.position.z,
+                      newPose.pose.orientation.x, newPose.pose.orientation.y, newPose.pose.orientation.z, newPose.pose.orientation.w);
+            if (firstIK) {
+                rdata->kine_state->setVariablePositions(jStartPos);
+                for (auto jj : rdata->kine_state->getVariableNames())
+                   ROS_DEBUG("jointname: %s", jj.c_str());
+                rdata->kine_state->copyJointGroupPositions(rdata->joint_modelGroup, jStartPos);
+                rdata->kine_state->updateLinkTransforms();
+                ROS_DEBUG_STREAM("Cart Start Position: " << rdata->kine_state->getGlobalLinkTransform("ee_link").translation() << std::endl);
+                ROS_DEBUG_COND(true, "Joint Start Position:  J1:%6.3f, J2:%6.3f, J3:%6.3f \n\
+                                                         J4:%6.3f, J5:%6.3f, J6:%6.3f",
+                                                         jStartPos[0], jStartPos[1],jStartPos[2],
+                                                         jStartPos[3], jStartPos[4],jStartPos[5]);
+                rdata->kine_state->setJointGroupPositions(rdata->joint_modelGroup, jStartPos);
+                firstIK = false;
+            }
+            foundIK = rdata->kine_state->setFromIK(rdata->joint_modelGroup, newPose.pose, attempts, timeout);
+            if (foundIK){
+                rdata->kine_state->copyJointGroupPositions(rdata->joint_modelGroup, jPos);
+                for (std::size_t i = 0; i < rdata->joint_names.size(); ++i) {
+                      ROS_DEBUG_COND(false,"Joint %s: %f", rdata->joint_names[i].c_str(), rad2deg(jPos[i]));
+                }
+                pubs->at("trj_pub")->publish(composeTrjMessage(jPos));
+            } else {
+                ROS_WARN("No IK solution found");
+            }
+        } else if (pos->isJoint) {
+            lastjtPose = jtPose;
+            ROS_WARN_THROTTLE(1, "Joint publish not yet implemented");
+        } else {
+            ROS_ERROR_THROTTLE(1, "No jog info to publish");
+        }
+        pos->waitIK = false;
+    }
+}
+
+////////////////////////////////////////
+//////////////// MAIN //////////////////
+////////////////////////////////////////
+int main(int argc, char **argv) {
+    /// node variables ///
+    int loopRate, msgBufferSize, jogPubRate;
+    std::string param_ns;
+    ROS_topics ros_topics;
+    /// internal state struct ////
+    internalState is;
+    positionData pos_data;
+    robotData robot_data;
+    auxData aux_data;
+    /// setup node parameters ///
+    ros::init(argc, argv, "roboglue_follower");
+    ros::NodeHandle nh;
+    ///read parameters from server ///
+    nh.getParam("/roboglue_ros_relay/parameter_ns", param_ns);
+    nh.getParam(param_ns+"loop_rate", loopRate);
+    nh.getParam(param_ns+"msg_buffer", msgBufferSize);
+    nh.getParam(param_ns+"INstate", ros_topics.stateSub);
+    nh.getParam(param_ns+"INcommands", ros_topics.commandSub);
+    nh.getParam(param_ns+"INcoordinates", ros_topics.coordSub);
+    nh.getParam(param_ns+"OUTstate", ros_topics.statePub);
+    nh.getParam(param_ns+"OUTcommands", ros_topics.commandPub);
+    nh.getParam(param_ns+"OUTcoordinates", ros_topics.coordPub);
+    nh.getParam(param_ns+"OUTdelta_jog", ros_topics.delta_jog);
+    nh.getParam(param_ns+"OUTdelta_joint_jog", ros_topics.delta_joint_jog);
+    nh.getParam(param_ns+"OUTtraj_jog", ros_topics.traj_jog);
+    nh.getParam(param_ns+"robot_model_name", robot_data.robot_model_name);
+    nh.getParam(param_ns+"move_group_name", robot_data.move_group_name);
+    nh.getParam("/roboglue_ros_follower/jog_pub_rate", jogPubRate);
+
+    /// set spinner rate ///
+    ros::Rate loop_rate(loopRate);
+    /// set console log level ///
+    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
+    ROS_INFO("Follower Node Started");
+
+    /// advertise publish topics ///
+    publisherMap publishers;
+    ros::Publisher jog_pub = nh.advertise<geometry_msgs::TwistStamped>(ros_topics.delta_jog,
+                                                                       static_cast<uint32_t>(msgBufferSize));
+    ros::Publisher jog_joint_pub = nh.advertise<geometry_msgs::TwistStamped>(ros_topics.delta_joint_jog,
+                                                                             static_cast<uint32_t>(msgBufferSize));
+    ros::Publisher trj_pub = nh.advertise<trajectory_msgs::JointTrajectory>(ros_topics.traj_jog,
+                                                                            static_cast<uint32_t>(msgBufferSize));
+    /// build a list of publisher to pass to mqtt callback ///
+    publishers["jog_pub"] = &jog_pub;
+    publishers["jog_joint_pub"] = &jog_joint_pub;
+    publishers["trj_pub"] = &trj_pub;
+    /// subscribe to incoming topics ///
+    ros::Subscriber command_sub = nh.subscribe<std_msgs::String>(ros_topics.commandSub, static_cast<uint32_t>(msgBufferSize),
+                                                                 boost::bind(commandCallback, _1, &is, &pos_data));
+    ros::Subscriber coordinate_sub = nh.subscribe<std_msgs::String>(ros_topics.coordSub, static_cast<uint32_t>(msgBufferSize),
+                                                                    boost::bind(coordinateCallback, _1, &publishers, &pos_data));
+    ros::Subscriber state_sub = nh.subscribe<std_msgs::String>(ros_topics.stateSub, static_cast<uint32_t>(msgBufferSize),
+                                                               boost::bind(stateCallback, _1, &is, &aux_data));
+    /// load the robot model for inverse kinematics and self collision checking ///
+    /// requires parameter server and moveIT to be active
+    moveit::planning_interface::MoveGroupInterface move_group(robot_data.move_group_name);
+    robot_data.kine_model = move_group.getRobotModel();
+    robot_data.kine_state = std::make_shared<robot_state::RobotState>(robot_data.kine_model);
+    robot_data.joint_modelGroup = robot_data.kine_state->getJointModelGroup(robot_data.move_group_name);
+    robot_data.joint_names = robot_data.joint_modelGroup->getVariableNames();
+    ROS_INFO("Planning in Frame: %s for link: %s", move_group.getPlanningFrame().c_str(), move_group.getEndEffectorLink().c_str());
+    ROS_INFO("IK calc in Frame: %s", robot_data.kine_model->getModelFrame().c_str());
+
+    /// create timed callbacks ///
+    ros::Timer timedPublish = nh.createTimer(ros::Duration(1.0/static_cast<double>(jogPubRate)),
+                                             boost::bind(timedPublishCallback, _1, &is, &publishers, &pos_data, &robot_data));
+    // now we can get inverse kinematics from kinematic state using the joint model
+
+    while (ros::ok() && is.isRunning) {
+        ROS_DEBUG_ONCE("Follower Node Looping");
+        //testFunction(&trj_pub);
+        ros::spinOnce();
+        //if (is.isRealtime)
+        loop_rate.sleep();
+    }
+    return 0;
+}
+////////////////////////////////////////
+///////////// END MAIN /////////////////
+////////////////////////////////////////
+
+trajectory_msgs::JointTrajectory composeTrjMessage(std::vector<double> jtpos){
+    static trajectory_msgs::JointTrajectory trj;
+    static double ang=0.0;
+    trj.header.seq++;
+    trj.header.stamp = ros::Time::now();
+    trj.points.clear();
+    trj.joint_names = {"shoulder_pan_joint","shoulder_lift_joint","elbow_joint","wrist_1_joint","wrist_2_joint","wrist_3_joint"};
+    trajectory_msgs::JointTrajectoryPoint pt;
+    pt.positions = jtpos;
+    pt.time_from_start.fromSec(0.5);
+    ang += 0.1;
+    trj.points.push_back(pt);
+    return trj;
+}
+
+////////////////////////////////////////
+///////////// TEST FUNCTIONS ///////////
+////////////////////////////////////////
+void testFunction(ros::Publisher* trj_pub){
+    static trajectory_msgs::JointTrajectory trj;
+    static double ang=0.0;
+    trj.header.seq++;
+    trj.header.stamp = ros::Time::now();
+    trj.points.clear();
+    trj.joint_names = {"shoulder_pan_joint","shoulder_lift_joint","elbow_joint","wrist_1_joint","wrist_2_joint","wrist_3_joint"};
+    trajectory_msgs::JointTrajectoryPoint pt;
+    pt.positions = {M_PI_2+M_PI_4*sin(2*ang), -3*M_PI_4, 3*M_PI_4-M_PI_4*0.65*sin(ang), -M_PI_2-M_PI_4, -M_PI_2, 0};
+    pt.time_from_start.fromSec(0.5);
+    ang += 0.1;
+    trj.points.push_back(pt);
+    trj_pub->publish(trj);
+}
+
+

src/roboglue_ros/src/roboglue_recorder.cpp

@@ -0,0 +1,678 @@
+/*
+20190219 - Inizia la scrittura del nodo che si occupa di registrare e riprodurre i file con i percorsi.
+20190307 - Prima implementazione della scirttura del file metadati con le informazoni
+            sul file percorso da eseguire.
+20190308 - Lettura e Scrittura dei file di template dei metadati e dei dati di percorso,
+            inizio implementazione della parte nella callback delle coordinate che deve
+            raccogliere e memorizzare i dati a seconda che sia tempo o distanza il metodo prescelto.
+20190311 - Scrittura in memoria delle coordinate in arrivo in unvettore di struct che mantengono tutte le info
+            di una riga di dati compresi gli ingressi ausiliari.
+            Prima scrittuta della callback per la ricezione dei cambiamenti di stato
+            degli I/O digitali e analogici
+20190322 - Terminata la scrittura delle procedure per salvataggio e ripristino delle coordinate
+            salvate da memoria in un file CSV. Vengono usati due template:
+            1) compilazione del file dei metadati, contiene informazioni sul contenuto del file
+            delle coordinate e su come va interpretato.
+            2) file dei punti in CSV, contine coordinate e inormazioni per essere interpretato
+            sia nell'invio di coordinate a tempo costante o a distanza costante
+            Ricordarsi in caso di modifiche alla struttura del file delle coordinate di aggiornare
+            l' enum VPOS con i nomi delle colonne aggiunte.
+20190325 - Implementato uso del ros parameter server per la configurazione del nodo.
+            Contiene i nomi delle code ros.
+20190328 - Prima implementazione dei planner di moveit per la generazione delle traiettorie.
+            Le traiettorie generate non rispettano la velocità di esecuzione richiesta, vanno elaborate
+            dopo la pianificazione per assegnare un profilo derivato dai dati registrati nel file.
+20190402 - Primi tentativi di pianificazione di percorso da file. La traiettoria viene pianificata
+            correttamente da moveit come path cartesiano ma l'esecuzione fallisce per un errore
+            interno del controller di ros. Risultato comunque positivo. Corretti alcuni errori
+            di logica nella lettura del file che davano segfault nell'accesso ai vettori.
+20190404 - Riscritto il modo in cui vengono recuperati robot model e robot state partendo
+            solo dal movegroup senza usare il robot model loader.
+            Cerco i risolvere i problemi del salto dei giunti, da valutare la riscrittura del
+            planner per evitare queste cose o verificare durante la creazione del percorso
+            che non si incappi in configurazioni instabili.
+20190708 - Implementazione funzioni di visualizzazione del percorso e pulizia dello schermo prima
+            dell'esecuzione del file.
+*/
+////////// ROS INCLUDES /////////////
+#include "ros/ros.h"
+#include <roboglue_ros/roboglue_utils.h> /// utility function library
+#include <moveit/move_group_interface/move_group_interface.h>
+///////// STANDARD LIBRARIES ////////
+#include <iostream>
+#include <fstream>
+
+typedef struct {
+    nlohmann::json* metaFile = nullptr;
+    rapidcsv::Document* dataFile = nullptr;
+    std::string meta_dir_name;
+    std::string data_dir_name;
+    std::string meta_template_name;
+    std::string data_template_name;
+    std::string meta_ext;
+    std::string data_ext;
+    std::vector<pointRecord>* recordVect;
+    std::vector<pointRecord>* playVect;
+} fileData;
+
+// posizione degli elementi nella riga del csv
+enum VPOS {X,Y,Z,RX,RY,RZ,DT,DS,VMAX};
+
+bool openFile(internalState* is, fileData* fd, nlohmann::json meta);
+bool closeFile(internalState* is, fileData* fd);
+
+bool startRecord(internalState* is, fileData* fd,  nlohmann::json meta);
+bool stopRecord(internalState* is, fileData* fd);
+
+bool startPlay(internalState* is, fileData* fd,  nlohmann::json meta);
+bool stopPlay(internalState* is, fileData* fd);
+
+void plotPoints (rvizData* rvdata, fileData* fd);
+void plotTrajectory(rvizData* rvdata, robotData* robotdata, moveit_msgs::RobotTrajectory* traj);
+void clearTrajectory(rvizData* rvdata);
+
+void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, positionData* pos, rvizData* rvd, fileData* fd) {
+    ROS_DEBUG("Received command: [%s]", msg->data.c_str());
+    try {
+        nlohmann::json c = nlohmann::json::parse(msg->data.c_str());
+        std::string command = c["command"];
+        nlohmann::json params = c["params"];
+        nlohmann::json lock;
+        std::string action;
+        if (!command.compare("QUIT")){
+            is->isRunning = false;
+        } else if (!command.compare("OPEN")){
+            action = params["action"];
+            if (!action.compare("open")){
+                is->isFileOpen = openFile(is, fd, params["metadata"]);
+                if (params["metadata"]["plot"].get<bool>()){
+                    plotPoints(rvd, fd);
+                }
+            } else if (!action.compare("close")){
+                is->isFileOpen = !closeFile(is, fd);
+                clearTrajectory(rvd);
+            }
+        } else if (!command.compare("PLAY")){
+            action = params["action"];
+            if (!action.compare("start")){
+                is->isPlaying = startPlay(is, fd, params["metadata"]);
+            } else if (!action.compare("stop")) {
+                is->isPlaying = !stopPlay(is, fd);
+            }
+        } else if (!command.compare("RECORD")){
+            action = params["action"];
+            if (!action.compare("start")){
+                getLockInfo(pos, params["lock"]);
+                pos->isFirst = true;
+                is->isRecording = startRecord(is, fd, params["metadata"]);
+            } else if (!action.compare("stop") ) {
+                is->isRecording = !stopRecord(is, fd);
+            }
+        } else if (!command.compare("REALTIME")) {
+            action = params["action"];
+            if (!action.compare("start")){
+                getLockInfo(pos, params["lock"]);
+                is->isRealtime=true;
+                is->isPlaying=false;
+                is->isRecording=false;
+            } else if (!action.compare("stop")){
+                is->isRealtime=false;
+                is->isPlaying=false;
+                is->isRecording=false;
+            }
+        } else {
+            ROS_WARN("Unknown command: %s", command.c_str());
+        }
+    } catch (nlohmann::json::exception &e) {
+        ROS_ERROR("Failed to parse command: [%s]", e.what());
+        is->isPlaying = false;
+        is->isRealtime = false;
+        is->isRecording = false;
+    }
+}
+
+void coordinateCallback(const std_msgs::String::ConstPtr& msg, internalState* is, positionData* pos) {
+    ROS_DEBUG("Received coordinate: [%s]", msg->data.c_str());
+    nlohmann::json coordinateParsed = nlohmann::json::parse(msg->data);
+    std::string type = coordinateParsed["type"].get<std::string>();
+    std::string mode = coordinateParsed["mode"].get<std::string>();
+    geometry_msgs::TwistStamped twPose;
+    trajectory_msgs::JointTrajectory jtPose;
+    static geometry_msgs::TwistStamped twPoseMem;
+    static trajectory_msgs::JointTrajectory jtPoseMem;
+
+    if (is->isRecording || is->isRealtime){
+        if (!type.compare("cartesian")){
+            nlohmann::json coordinateValue = coordinateParsed["value"];
+            pos->isTwist = true;
+            pos->isJoint = false;
+            if(!mode.compare("pos")){
+                pos->isPos = true;
+                pos->isVel = false;
+                twPose = jstr2Twist(coordinateParsed["value"]);
+                twPose = lockTwistPose(twPose, twPoseMem, *pos);
+                twPoseMem = twPose;
+                pos->twist = twPose;
+            } else if (!mode.compare("vel")) {
+                pos->isPos = false;
+                pos->isVel = true;
+            }
+            pos->isNew = true;
+        } else if (!type.compare("joint")){
+            pos->isTwist = false;
+            pos->isJoint = true;
+            if(!mode.compare("pos")){
+                pos->isPos = true;
+                pos->isVel = false;
+                jtPose = jstr2Joint(coordinateParsed["value"]);
+                jtPoseMem = jtPose;
+                pos->joint = jtPose;
+            } else if (!mode.compare("vel")) {
+                pos->isPos = false;
+                pos->isVel = true;
+            }
+            pos->isNew = true;
+        } else {
+            ROS_ERROR("Unknown coordinate type");
+        }
+    }
+}
+
+void stateCallback(const std_msgs::String::ConstPtr& msg, internalState*is, auxData* aux){
+    ROS_DEBUG("Received aux I/O State Update: [%s]", msg->data.c_str());
+    nlohmann::json auxParsed = nlohmann::json::parse(msg->data);
+    nlohmann::json digitalParsed = auxParsed["digital"];
+    nlohmann::json analogParsed = auxParsed["analog"];
+    std::vector<std::pair<std::string,bool>> tempDigital;
+    std::vector<std::pair<std::string,uint16_t>> tempAnalog;
+    if (is->isRecording || is->isRealtime){
+        for (auto di : digitalParsed.items()){
+            tempDigital.push_back(std::pair<std::string,bool>(di.key(),di.value().get<bool>()));
+        }
+        for (auto ai : analogParsed.items()){
+            tempAnalog.push_back(std::pair<std::string,uint16_t>(ai.key(),ai.value().get<uint16_t>()));
+        }
+        aux->digital.clear();
+        aux->analog.clear();
+        aux->digital.assign(tempDigital.begin(), tempDigital.end());
+        aux->analog.assign(tempAnalog.begin(), tempAnalog.end());
+    }
+}
+
+////////////////////////////////////////
+//////////////// MAIN //////////////////
+////////////////////////////////////////
+int main(int argc, char **argv) {
+    /// node variables ////
+    int loopRate, msgBufferSize;
+    double nonzero_move;
+    std::string param_ns;
+    ROS_topics ros_topics;
+    /// internal node state variables ///
+    internalState is;
+    auxData aux_data;
+    positionData pos_data;
+    robotData robot_data;
+    rvizData rviz_data;
+    fileData file_data;
+    std::string opMode;
+    pointRecord currentPoint;
+    std::vector<pointRecord> recordVector;
+    std::vector<pointRecord> playVector;
+    file_data.recordVect = &recordVector;
+    file_data.playVect = &playVector;
+
+    /// player variables ///
+    double dsCounter, dtCounter;
+    std::vector<pointRecord> tempPlayVector;
+    std::vector<geometry_msgs::PoseStamped> tempPlanningVector;
+    moveit::planning_interface::MoveGroupInterface::Plan jointPlan, pathPlan;
+    size_t i=0;
+    /// setup node parameters ///
+    ros::init(argc, argv, "roboglue_recorder");
+    ros::NodeHandle nh;
+    /// read parameters from server ///
+    //load common parameters
+    nh.getParam("/roboglue_ros_recorder/parameter_ns", param_ns);
+    nh.getParam(param_ns+"loop_rate", loopRate);
+    nh.getParam(param_ns+"msg_buffer", msgBufferSize);
+    nh.getParam(param_ns+"min_nonzero_move", nonzero_move);
+    nh.getParam(param_ns+"INstate", ros_topics.stateSub);
+    nh.getParam(param_ns+"INcommands", ros_topics.commandSub);
+    nh.getParam(param_ns+"INcoordinates", ros_topics.coordSub);
+    nh.getParam(param_ns+"OUTstate", ros_topics.statePub);
+    nh.getParam(param_ns+"OUTcommands", ros_topics.commandPub);
+    nh.getParam(param_ns+"OUTcoordinates", ros_topics.coordPub);
+    // load spin rate
+    nh.getParam("/roboglue_ros_recorder/loop_rate", loopRate);
+    //load meta template filename
+    nh.getParam("/roboglue_ros_recorder/meta_dir", file_data.meta_dir_name);
+    nh.getParam("/roboglue_ros_recorder/meta_template", file_data.meta_template_name);
+    // load data template filename
+    nh.getParam("/roboglue_ros_recorder/data_dir", file_data.data_dir_name);
+    nh.getParam("/roboglue_ros_recorder/data_template", file_data.data_template_name);
+    //load meta and data file extension
+    nh.getParam("/roboglue_ros_recorder/meta_ext", file_data.meta_ext);
+    nh.getParam("/roboglue_ros_recorder/data_ext", file_data.data_ext);
+    // load coordinate send operation mode (time/distance)
+    nh.getParam("/roboglue_ros_recorder/point_group_mode", robot_data.point_group_mode);
+    nh.getParam("/roboglue_ros_recorder/planning_mode", robot_data.planning_mode);
+    // load robot model information
+    nh.getParam(param_ns+"robot_model_name", robot_data.robot_model_name);
+    nh.getParam(param_ns+"move_group_name", robot_data.move_group_name);
+
+    /// set console log level ///
+    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug);
+    /// set spinner rate ///
+    ros::Rate loop_rate(loopRate);
+
+    ROS_INFO("Recorder Node Started");
+
+    ros::Subscriber command_sub = nh.subscribe<std_msgs::String>(ros_topics.commandSub, static_cast<uint32_t>(msgBufferSize),
+                                                                 boost::bind(commandCallback, _1, &is, &pos_data, &rviz_data, &file_data));
+    ros::Subscriber coordinate_sub = nh.subscribe<std_msgs::String>(ros_topics.coordSub, static_cast<uint32_t>(msgBufferSize),
+                                                                    boost::bind(coordinateCallback, _1, &is, &pos_data));
+    ros::Subscriber state_sub = nh.subscribe<std_msgs::String>(ros_topics.stateSub, static_cast<uint32_t>(msgBufferSize),
+                                                               boost::bind(stateCallback, _1, &is, &aux_data));
+    /// advertise publish topics ///
+    publisherMap publishers;
+    ros::Publisher command_pub = nh.advertise<std_msgs::String>(ros_topics.commandPub,
+                                                                static_cast<uint32_t>(msgBufferSize));
+    ros::Publisher coord_pub = nh.advertise<std_msgs::String>(ros_topics.coordPub,
+                                                              static_cast<uint32_t>(msgBufferSize));
+    ros::Publisher state_pub = nh.advertise<std_msgs::String>(ros_topics.statePub,
+                                                              static_cast<uint32_t>(msgBufferSize));
+
+    /// build a list of publisher to pass to mqtt callback ///
+    publishers[command_pub.getTopic()] = &command_pub;
+    publishers[coord_pub.getTopic()] = &coord_pub;
+    publishers[state_pub.getTopic()] = &state_pub;
+
+    /// load the robot model for inverse kinematics and self collision checking ///
+    /// requires parameter server and moveIT to be active
+    moveit::planning_interface::MoveGroupInterface move_group(robot_data.move_group_name);
+    robot_data.kine_model = move_group.getRobotModel();
+    robot_data.kine_state = std::make_shared<robot_state::RobotState>(robot_data.kine_model);
+    robot_data.joint_modelGroup = robot_data.kine_state->getJointModelGroup(robot_data.move_group_name);
+    robot_data.joint_names = robot_data.joint_modelGroup->getVariableNames();
+    ROS_INFO("Planning in Frame: %s for link: %s", move_group.getPlanningFrame().c_str(), move_group.getEndEffectorLink().c_str());
+    ROS_INFO("IK calc in Frame: %s", robot_data.kine_model->getModelFrame().c_str());
+
+    /// setup rviz palnned path visualization
+    rviz_data.vtools = std::make_shared<mvt::MoveItVisualTools>("base_link", "rviz_visual_tools", robot_data.kine_model);
+
+    bool startCycle = true;
+    while (ros::ok() && is.isRunning){
+        if (startCycle){
+            startCycle = false;
+            // clear visual interface extraneous objects
+            rviz_data.vtools->loadRemoteControl();
+            rviz_data.vtools->deleteAllMarkers();
+            rviz_data.vtools->trigger();
+        }
+        ros::spinOnce();
+          /////////////////////////////////////////
+         /////////// PLAY FILE ///////////////////
+        /////////////////////////////////////////
+        if (is.isPlaying){
+            // i -> row counter for temp vector planning in both cartesian and joint mode
+            ROS_INFO_COND(i==0,"Start Play file: [%s]", file_data.metaFile->at("name").get<std::string>().c_str());
+            if (i < playVector.size()){
+                tempPlayVector.clear();
+                dsCounter = 0.0;
+                // fill temporary vector to start planning (distance or time)
+                if (!robot_data.point_group_mode.compare("dist")){
+                    while (i<playVector.size() && ((dsCounter+=playVector[i].dS) < file_data.metaFile->at("ds").get<double>())) {
+                        tempPlayVector.push_back(playVector.at(i));
+                        i++;
+                    }
+                    ROS_DEBUG("Built Temp Play Vector; mode:[%s], size:[%u], dist:[%4.1f]", robot_data.point_group_mode.c_str(),
+                                                                                            tempPlayVector.size(), dsCounter);
+                } else if (!robot_data.point_group_mode.compare("time")) {
+                    while (i<playVector.size() && ((dtCounter+=playVector[i].point.header.stamp.toSec()) < file_data.metaFile->at("dt").get<double>())){
+                        tempPlayVector.push_back(playVector.at(i));
+                        i++;
+                    }
+                    ROS_DEBUG("Built Temp Play Vector; mode:[%s], size:[%u], time:[%4.1f]", robot_data.point_group_mode.c_str(),
+                                                                                            tempPlayVector.size(), dtCounter);
+                } else {
+                    ROS_ERROR("Invalid Point Group Mode!");
+                    stopPlay(&is, &file_data);
+                    ros::shutdown();
+                }
+                // convert twist to pose for the planner
+                tempPlanningVector.clear();
+                for (auto cc : tempPlayVector) {
+                    tempPlanningVector.push_back(twist2Pose(cc.point));
+                    tempPlanningVector.back().header.frame_id="/world";
+                }
+                //TODO sincronizzazione delle uscite ausiliarie con il movimento del robot!!!
+                // select right planner
+                if (!robot_data.planning_mode.compare("path")){
+                    const double jmp_thr= 0.00;
+                    const double ee_step= 0.05;
+                    std::vector<geometry_msgs::Pose> pss;
+                    moveit_msgs::RobotTrajectory traj;
+                    moveit_msgs::MoveItErrorCodes ec;
+                    move_group.setMaxVelocityScalingFactor(0.1);
+                    move_group.setPlanningTime(5.0);
+                    move_group.setGoalPositionTolerance(0.1);
+                    // create vector of poses only for the planner
+                    for (auto pp: tempPlanningVector) pss.push_back(pp.pose);
+                    move_group.setStartStateToCurrentState();
+                    move_group.computeCartesianPath(pss,ee_step,jmp_thr,traj, false, &ec);
+                    ROS_DEBUG("Trajectory Compute ExitCode: %d",ec.val);
+//                    BUG il controllo del salto dei giunti crasha il nodo.
+//                    if (checkJointJump(boost::shared_ptr<moveit_msgs::RobotTrajectory>(&traj), 90.0)){
+//                        ROS_WARN("Joint Jump Detected!!!");
+//                    }
+                    jointPlan.trajectory_ = traj;
+                    plotTrajectory(&rviz_data, &robot_data, &traj);
+                    // async execute planned trajectory
+                    moveit::planning_interface::MoveItErrorCode retval = move_group.execute(jointPlan);
+                    ROS_INFO("Finished executing first [%u] datapoints, execute: %d", i, retval.val);
+                } else if (!robot_data.planning_mode.compare("joint")){
+                    // TODO SCRIVERE LA LOGICA PER IL PERCORSO JOINT QUANDO
+                    // QUANDO I PROBLEMI DI STABILITÀ DEL MOVIMENTO SONO RISOLTI
+                } else {
+                    ROS_ERROR("Invalid Plannning Mode!");
+                    is.isPlaying = !stopPlay(&is, &file_data);
+                    ros::shutdown();
+                }
+            } else {
+                ROS_INFO("Stop Play file: [%s]", file_data.metaFile->at("name").get<std::string>().c_str());
+                is.isPlaying = !stopPlay(&is, &file_data);
+                i=0; //reset dS/dT counter
+            }
+        }
+          /////////////////////////////////////////
+         /////////// RECORD FILE /////////////////
+        /////////////////////////////////////////
+        if (is.isRecording && pos_data.isNew){
+            ROS_DEBUG("Registering New DataPoint");
+            // scrittura asincrona delle coordinate ricevute
+            pos_data.isNew = false;
+            if (pos_data.isFirst){
+                recordVector.clear();
+                playVector.clear();
+                currentPoint.pointNumber = 0;
+                currentPoint.dS = 0.0;
+            } else {
+                // record delta distance starting from 2nd point
+                if (recordVector.size() > 0)
+                    currentPoint.dS = e3dist(pos_data.twist.twist, recordVector.back().point.twist);
+                else
+                    currentPoint.dS = e3dist(pos_data.twist.twist, geometry_msgs::Twist());
+            }
+            if (currentPoint.dS > nonzero_move || pos_data.isFirst) {
+                pos_data.isFirst = false;
+                currentPoint.point =  pos_data.twist;
+                currentPoint.pointAux = aux_data;
+                currentPoint.pointNumber++;
+                recordVector.push_back(currentPoint);
+            }
+        }
+        loop_rate.sleep();
+    }
+    return 0;
+}
+////////////////////////////////////////
+///////////// END MAIN /////////////////
+////////////////////////////////////////
+
+bool openFile(internalState* is, fileData* fd, nlohmann::json meta){
+    if (!is->isFileOpen){
+        // open metafile by name
+        std::string metaInFilename = fd->meta_dir_name + meta["name"].get<std::string>() + fd->meta_ext;
+        ROS_DEBUG("Opening Meta File: [%s]", metaInFilename.c_str());
+        std::ifstream metaIn(metaInFilename);
+        if (metaIn.good()){
+            try  {
+                if(fd->metaFile == nullptr) fd->metaFile = new nlohmann::json;
+                metaIn >> *fd->metaFile;
+                metaIn.close();
+            } catch (nlohmann::json::exception &e) {
+                ROS_ERROR("Failed to Parse Meta File File: [%s]", e.what());
+                return false;
+            }
+        } else {
+            ROS_ERROR("Failed to open Meta File: failbit:%d badbit:%d",
+                      metaIn.failbit, metaIn.badbit);
+            return false;
+        }
+        // open data file
+        std::string dataInFilename = fd->data_dir_name + fd->metaFile->at("name").get<std::string>() + fd->data_ext;
+        ROS_DEBUG("Opening Data File: [%s]", dataInFilename.c_str());
+        std::ifstream dataIn(dataInFilename);
+        if (dataIn.good()){
+            if (fd->dataFile == nullptr) fd->dataFile = new rapidcsv::Document(dataIn,
+                                                                               rapidcsv::LabelParams(),
+                                                                               rapidcsv::SeparatorParams());
+            ROS_DEBUG("Open Data File of [%u] cols named [%s] and [%u] datapoints", fd->dataFile->GetColumnCount(),
+                                                                boost::algorithm::join(fd->dataFile->GetColumnNames(), ",").c_str(),
+                                                                fd->dataFile->GetRowCount());
+            dataIn.close();
+            // parse row to extract datapoints
+            std::vector<std::string> unpRow;
+            pointRecord pointRec;
+            std::vector<std::string> colNames = fd->dataFile->GetColumnNames();
+            size_t dataSize = fd->dataFile->GetRowCount();
+            for (size_t i=0; i<dataSize; i++){
+                unpRow = fd->dataFile->GetRow<std::string>(i);
+                pointRec.point.twist.linear.x = std::stod(unpRow.at(X));
+                pointRec.point.twist.linear.y = std::stod(unpRow.at(Y));
+                pointRec.point.twist.linear.z = std::stod(unpRow.at(Z));
+                pointRec.point.twist.angular.x = std::stod(unpRow.at(RX));
+                pointRec.point.twist.angular.y = std::stod(unpRow.at(RY));
+                pointRec.point.twist.angular.z = std::stod(unpRow.at(RZ));
+                pointRec.point.header.stamp.fromSec(std::stod(unpRow.at(DT)));
+                pointRec.point.header.seq = static_cast<unsigned int>(i);
+                pointRec.dS = std::stod(unpRow.at(DS));
+                for (size_t d = 0; d < fd->metaFile->at("n_digital").get<size_t>(); d++){
+                    std::pair<std::string, bool> di;
+                    di.first = colNames.at(VMAX+d);
+                    di.second = std::stoi(unpRow.at(VMAX+d));
+                    pointRec.pointAux.digital.push_back(di);
+                }
+                for (size_t a = 0; a < fd->metaFile->at("n_analog"); a++){
+                    std::pair<std::string, uint16_t> an;
+                    an.first = colNames.at(VMAX+fd->metaFile->at("n_digital").get<size_t>()+a);
+                    an.second = static_cast<uint16_t>(std::stoi(unpRow.at(VMAX+fd->metaFile->at("n_digital").get<size_t>()+a)));
+                    pointRec.pointAux.analog.push_back(an);
+                }
+                fd->playVect->push_back(pointRec);
+            }
+            ROS_DEBUG("Loaded [%u] datapoints", fd->playVect->size());
+            return  true;
+        } else {
+            ROS_ERROR("Failed to open Data File, failbit:%d badbit:%d", dataIn.failbit, dataIn.badbit);
+            return false;
+        }
+    }
+}
+
+bool closeFile(internalState* is, fileData* fd){
+    if (is->isFileOpen){
+        if (fd->dataFile != nullptr) {
+            delete fd->dataFile;
+            fd->dataFile = nullptr;
+        }
+        if (fd->metaFile != nullptr) {
+            delete fd->metaFile;
+            fd->metaFile = nullptr;
+        }
+       fd->playVect->clear();
+       fd->recordVect->clear();
+    }
+}
+
+bool startRecord(internalState* is, fileData* fd, nlohmann::json meta){
+    if (!is->isRecording){
+        // open metafile template
+        std::string metaInFilename = fd->meta_dir_name + fd->meta_template_name;
+        ROS_DEBUG("Opening Meta File Template: [%s]", metaInFilename.c_str());
+        std::ifstream metaIn(metaInFilename);
+        if (metaIn.good()){
+            try {
+                if (fd->metaFile == nullptr) fd->metaFile = new nlohmann::json;
+                metaIn >> *fd->metaFile;                
+                fd->metaFile->at("name") = meta["name"].get<std::string>();
+                fd->metaFile->at("code") = meta["code"].get<std::string>();
+                fd->metaFile->at("ds") = meta["ds"].get<double>();
+                fd->metaFile->at("dt") = meta["dt"].get<double>();
+            } catch (nlohmann::json::exception &e){
+                ROS_ERROR("Failed to parse Meta File Template: [%s]", e.what());
+                return false;
+            }
+            metaIn.close();
+        } else {
+            ROS_ERROR("Failed to open Meta File Template failbit:%d badbit:%d",
+                      metaIn.failbit, metaIn.badbit);
+            return false;
+        }
+        // open datafile template
+        std::string dataInFilename = fd->data_dir_name + fd->data_template_name;
+        ROS_DEBUG("Opening Data File Template: {%s]", dataInFilename.c_str());
+        std::ifstream dataIn(dataInFilename);
+        if (dataIn.good()) {
+            if(fd->dataFile == nullptr) fd->dataFile = new rapidcsv::Document(dataIn,
+                                                  rapidcsv::LabelParams(),
+                                                  rapidcsv::SeparatorParams());
+            fd->dataFile->RemoveRow(0);
+            ROS_DEBUG("Open Data File of [%u] cols named [%s]", fd->dataFile->GetColumnCount(),
+                                                            boost::algorithm::join(fd->dataFile->GetColumnNames(), ",").c_str());
+           dataIn.close();
+        } else {
+            ROS_ERROR("Failed to open Data File Template failbit:%d badbit:%d", metaIn.failbit, metaIn.badbit);
+            return false;
+        }
+    }
+    return true;
+}
+
+bool stopRecord(internalState* is, fileData* fd){
+    size_t rowIndex=0;
+    size_t vectSize = fd->recordVect->size();
+    std_msgs::Header firstStamp;
+    geometry_msgs::Twist dPoint;
+    if (is->isRecording) {
+        if (!fd->recordVect->empty()) {
+            if (fd->recordVect->front().pointAux.digital.size() > 0)
+                for (auto di = fd->recordVect->front().pointAux.digital.begin(); di != fd->recordVect->front().pointAux.digital.end(); ++di){
+                    std::string dname = di->first;
+                    fd->dataFile->AppendColumn(di->first, std::vector<std::string>({}));
+                }
+            if (fd->recordVect->front().pointAux.analog.size() > 0)
+                for (auto ai = fd->recordVect->front().pointAux.analog.begin(); ai != fd->recordVect->front().pointAux.analog.end(); ++ai){
+                    std::string aname = ai->first;
+                    fd->dataFile->AppendColumn(ai->first, std::vector<std::string>({}));
+                }
+            ROS_DEBUG("Template File contains: [%d] rows", fd->dataFile->GetRowCount());
+            // get first timestamp to get relative dt between points
+            firstStamp.stamp = fd->recordVect->front().point.header.stamp;
+            // fill dataFile with pointRecord Vector
+            std::vector<std::string> cRow;
+            for(rowIndex=0; rowIndex < vectSize; rowIndex++){
+                cRow.resize(VMAX);
+                cRow.at(X) = std::to_string(fd->recordVect->at(rowIndex).point.twist.linear.x);
+                cRow.at(Y) = std::to_string(fd->recordVect->at(rowIndex).point.twist.linear.y);
+                cRow.at(Z) = std::to_string(fd->recordVect->at(rowIndex).point.twist.linear.z);
+                cRow.at(RX) = std::to_string(fd->recordVect->at(rowIndex).point.twist.angular.x);
+                cRow.at(RY) = std::to_string(fd->recordVect->at(rowIndex).point.twist.angular.y);
+                cRow.at(RZ) = std::to_string(fd->recordVect->at(rowIndex).point.twist.angular.z);
+                cRow.at(DT) = std::to_string((fd->recordVect->at(rowIndex).point.header.stamp-firstStamp.stamp).toSec());
+                cRow.at(DS) = std::to_string(fd->recordVect->at(rowIndex).dS);
+                if (fd->recordVect->front().pointAux.digital.size() > 0)
+                    for(auto di = fd->recordVect->at(rowIndex).pointAux.digital.begin(); di != fd->recordVect->at(rowIndex).pointAux.digital.end(); ++di){
+                        cRow.push_back(std::to_string(di->second));
+                    }
+                if (fd->recordVect->front().pointAux.analog.size() > 0)
+                    for (auto ai = fd->recordVect->at(rowIndex).pointAux.analog.begin(); ai != fd->recordVect->at(rowIndex).pointAux.analog.end(); ++ai){
+                        cRow.push_back(std::to_string(ai->second));
+                    }
+                fd->dataFile->AppendRow("N"+std::to_string(rowIndex),cRow);
+                cRow.clear();
+            }
+            // save informations in metafile
+            fd->metaFile->at("n_point") = fd->dataFile->GetRowCount();
+            fd->metaFile->at("n_digital") = fd->recordVect->front().pointAux.digital.size();
+            fd->metaFile->at("n_analog") = fd->recordVect->front().pointAux.analog.size();
+            // save datafile
+            std::string dataOutFilename = fd->data_dir_name + fd->metaFile->at("name").get<std::string>() + ".data";
+            ROS_DEBUG("Saving Data File: %s", dataOutFilename.c_str());
+            std::ofstream dataOut (dataOutFilename);
+            if (dataOut.good()){
+                fd->dataFile->Save(dataOut);
+                dataOut.close();
+                ROS_DEBUG("Saving Data File with [%d] colums named [%s] and [%d] datapoints",
+                          fd->dataFile->GetColumnCount(), boost::algorithm::join(fd->dataFile->GetColumnNames(), ",").c_str(), fd->dataFile->GetRowCount());
+            } else {
+                ROS_ERROR("Failed to save Data File: [%s] failbit:%d badbit:%d", dataOutFilename.c_str(), dataOut.failbit, dataOut.badbit);
+                return false;
+            }
+            // save metafile
+            std::string metaOutFilename = fd->meta_dir_name + fd->metaFile->at("name").get<std::string>() + ".meta";
+            ROS_DEBUG("Saving Meta File: [%s]", metaOutFilename.c_str());
+            std::ofstream metaOut(metaOutFilename);
+            if (metaOut.good()) {
+                metaOut << *fd->metaFile << std::endl;
+                metaOut.close();
+            } else {
+                ROS_ERROR("Failed to save Meta File: [%s] failbit:%d badbit:%d", metaOutFilename.c_str(), metaOut.failbit, metaOut.badbit);
+                return false;
+            }
+        } else {
+            ROS_WARN("Record Vector is empty, cannot save file");
+        }
+    } else {
+        ROS_WARN("Was Not Recording, file not saved");
+    }
+    if (fd->dataFile != nullptr) {
+        delete fd->dataFile;
+        fd->dataFile = nullptr;
+    }
+    if (fd->metaFile != nullptr) {
+        delete fd->metaFile;
+        fd->metaFile = nullptr;
+    }
+    fd->recordVect->clear();
+    fd->playVect->clear();
+    return true;
+}
+
+bool startPlay(internalState* is, fileData* fd, nlohmann::json meta){
+    if(!is->isPlaying && !is->isFileOpen)
+        return openFile(is,fd,meta);
+    else if (!is->isPlaying && is->isFileOpen)
+        return true;
+}
+
+bool stopPlay(internalState* is, fileData* fd){
+    if (is->isPlaying && is->isFileOpen){
+        closeFile(is,fd);
+    }
+    return true;
+}
+
+void plotPoints (rvizData* rvdata, fileData* fd){
+    if (fd->playVect != nullptr){
+        for(auto point : *fd->playVect){
+            rvdata->vtools->publishSphere(twist2Pose(point.point).pose, rvt::RED, rvt::MEDIUM);
+        }
+        rvdata->vtools->publishSphere(twist2Pose(fd->playVect->front().point).pose, rvt::RED, rvt::LARGE);
+        rvdata->vtools->publishText(twist2Pose(fd->playVect->front().point).pose, "START", rvt::WHITE, rvt::LARGE);
+        rvdata->vtools->publishSphere(twist2Pose(fd->playVect->back().point).pose, rvt::RED, rvt::LARGE);
+        rvdata->vtools->publishText(twist2Pose(fd->playVect->back().point).pose, "END", rvt::WHITE, rvt::LARGE);
+        rvdata->vtools->trigger();
+    }
+}
+
+void plotTrajectory(rvizData* rvdata, robotData* robotdata, moveit_msgs::RobotTrajectory* traj) {
+    rvdata->vtools->publishTrajectoryLine(*traj, robotdata->joint_modelGroup, rvt::colors::BLUE);
+    rvdata->vtools->trigger();
+};
+
+void clearTrajectory(rvizData* rvdata){
+    rvdata->vtools->deleteAllMarkers();
+    rvdata->vtools->trigger();
+};

src/roboglue_ros/src/roboglue_relay.cpp

@@ -0,0 +1,165 @@
+/*
+20190212 - Inizio la scrittura del nodo che si interfaccia da un lato con RoboGlue e dall'altro con
+            ROS per l'invio di comandi dal programma al robot, recupero ed esecuzione di traiettorie
+            sia live che registrate in fie .bag da questo o un nodo parente.
+20190219 - Continua scrittura del nodo relay: la conversione dei tipi messaggio sarà rimandata ai
+            modi che li devono interpretare. qui viene trattato tutto come stringa.
+20190325 - Implementato uso del ros parameter server per la configurazione del nodo.
+            Contiene i nomi delle code sia ros che mqtt.
+*/
+
+////////// ROS INCLUDES /////////////
+#include "ros/ros.h"
+#include <roboglue_ros/roboglue_utils.h>
+
+#include "mqtt/async_client.h"
+
+////////////////////////////////////////
+///////////////MQTT CALLBACKS //////////
+////////////////////////////////////////
+class mqtt2rosClass: public virtual mqtt::callback {
+    std::map<std::string, ros::Publisher*> pubs_;
+    MQTT_topics mqtt_data_;
+    ROS_topics ros_data_;
+    public:
+        mqtt2rosClass(mqtt::async_client &cli, std::map<std::string, ros::Publisher*> pubs, MQTT_topics mdata, ROS_topics rdata){
+            pubs_=pubs;
+            mqtt_data_ = mdata;
+            ros_data_ = rdata;
+        }
+        void connected(const mqtt::string &cause) override {
+            ROS_INFO("MQTTclient Connected: %s", cause.c_str());
+        }
+        void connection_lost(const mqtt::string &cause) override {
+             ROS_INFO("MQTTclient Disconnected: %s", cause.c_str());
+        }
+        void delivery_complete(mqtt::delivery_token_ptr tok) override {
+             ROS_DEBUG("MessageDelivered, t:%d",tok->get_message_id());
+        }
+        void message_arrived(mqtt::const_message_ptr msg) override{
+            ROS_DEBUG("MqttMessage!");
+            std::string topic(msg->get_topic());
+            std::string payload(msg->get_payload_str());
+            std_msgs::String rosMsg;
+            rosMsg.data = payload;
+
+            if (!topic.compare(mqtt_data_.MQTTcommandSub)){
+                ROS_DEBUG("Command! -> %s", ros_data_.commandSub.c_str());
+                pubs_[ros_data_.commandSub]->publish(rosMsg);
+            } else if (!topic.compare(mqtt_data_.MQTTcoordSub)){
+                ROS_DEBUG("Coordinate! -> %s", ros_data_.coordSub.c_str());
+                pubs_[ros_data_.coordSub]->publish(rosMsg);
+            } else if (!topic.compare(mqtt_data_.MQTTstateSub)){
+                ROS_DEBUG("State! -> %s", ros_data_.stateSub.c_str());
+                pubs_[ros_data_.stateSub]->publish(rosMsg);
+            } else {
+                ROS_ERROR("Topic NOT Implemented");
+            }
+        }
+};
+
+////////////////////////////////////////
+/////////////// ROS CALLBACKS //////////
+////////////////////////////////////////
+void commandCallback(const std_msgs::String::ConstPtr msg, mqtt::async_client* cli_, MQTT_topics* mqtttopics){
+    ROS_DEBUG("RosMessage: Command!");
+    cli_->publish(mqtt::make_message(mqtttopics->MQTTcommandPub, msg->data.c_str()));
+}
+void poseCallback(const std_msgs::String::ConstPtr msg, mqtt::async_client* cli_, MQTT_topics* mqtttopics){
+    ROS_DEBUG("RosMessage: Pose!");
+    cli_->publish(mqtt::make_message(mqtttopics->MQTTcoordPub, msg->data.c_str()));
+}
+void stateCallback(const std_msgs::String::ConstPtr msg, mqtt::async_client* cli_, MQTT_topics* mqtttopics){
+    ROS_DEBUG("RosMessage: State!");
+    cli_->publish(mqtt::make_message(mqtttopics->MQTTstatePub, msg->data.c_str()));
+}
+
+////////////////////////////////////////
+//////////////// MAIN //////////////////
+////////////////////////////////////////
+int main(int argc, char **argv) {
+    /// node variables ///
+    int loopRate, msgBufferSize;
+    std::string param_ns;
+    MQTT_topics mqtt_topics;
+    ROS_topics ros_topics;
+    /// setup node parameters ///
+    ros::init(argc, argv, "roboglue_relay");
+    ros::NodeHandle nh;
+    ///read parameters from server ///
+    nh.getParam("/roboglue_ros_relay/parameter_ns", param_ns);
+    nh.getParam(param_ns+"loop_rate", loopRate);
+    nh.getParam(param_ns+"msg_buffer", msgBufferSize);
+    nh.getParam(param_ns+"INstate", ros_topics.stateSub);
+    nh.getParam(param_ns+"INcommands", ros_topics.commandSub);
+    nh.getParam(param_ns+"INcoordinates", ros_topics.coordSub);
+    nh.getParam(param_ns+"OUTstate", ros_topics.statePub);
+    nh.getParam(param_ns+"OUTcommands", ros_topics.commandPub);
+    nh.getParam(param_ns+"OUTcoordinates", ros_topics.coordPub);
+    nh.getParam("/roboglue_ros_relay/mqtt_INstate", mqtt_topics.MQTTstateSub);
+    nh.getParam("/roboglue_ros_relay/mqtt_INcommands", mqtt_topics.MQTTcommandSub);
+    nh.getParam("/roboglue_ros_relay/mqtt_INcoordinates", mqtt_topics.MQTTcoordSub);
+    nh.getParam("/roboglue_ros_relay/mqtt_OUTstate", mqtt_topics.MQTTstatePub);
+    nh.getParam("/roboglue_ros_relay/mqtt_OUTcommands", mqtt_topics.MQTTcommandPub);
+    nh.getParam("/roboglue_ros_relay/mqtt_OUTcoordinates", mqtt_topics.MQTTcoordPub);
+    nh.getParam("/roboglue_ros_relay/mqtt_host", mqtt_topics.mqttHost);
+    nh.getParam("/roboglue_ros_relay/mqtt_qos", mqtt_topics.mqttQoS);
+
+    /// set spinner rate ///
+    ros::Rate loop_rate(loopRate);
+    /// set console log level ///
+    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
+    ROS_INFO("Relay Node Started");
+    /// initialize MQTT client ///
+    mqtt::async_client client(mqtt_topics.mqttHost,"");
+
+    /// advertise publish topics ///
+    publisherMap publishers;
+    ros::Publisher command_pub = nh.advertise<std_msgs::String>(ros_topics.commandSub,static_cast<uint32_t>(msgBufferSize));
+    ros::Publisher pose_pub = nh.advertise<std_msgs::String>(ros_topics.coordSub, static_cast<uint32_t>(msgBufferSize));
+    ros::Publisher state_pub = nh.advertise<std_msgs::String>(ros_topics.stateSub,static_cast<uint32_t>(msgBufferSize));
+    /// build a list of publisher to pass to mqtt callback ///
+    publishers[command_pub.getTopic()] = &command_pub;
+    publishers[pose_pub.getTopic()] = &pose_pub;
+    publishers[state_pub.getTopic()] = &state_pub;
+    /// create mqtt callback ///
+    mqtt2rosClass mqtt2ros(client, publishers, mqtt_topics, ros_topics);
+
+    /// subscribe to incoming topics ///
+    ros::Subscriber command_sub = nh.subscribe<std_msgs::String>(ros_topics.commandPub,static_cast<uint32_t>(msgBufferSize),
+                                                                 boost::bind(commandCallback, _1, &client, &mqtt_topics));
+    ros::Subscriber pose_sub = nh.subscribe<std_msgs::String>(ros_topics.coordPub,static_cast<uint32_t>(msgBufferSize),
+                                                              boost::bind(poseCallback, _1, &client, &mqtt_topics));
+    ros::Subscriber state_sub = nh.subscribe<std_msgs::String>(ros_topics.statePub,static_cast<uint32_t>(msgBufferSize),
+                                                               boost::bind(stateCallback, _1, &client, &mqtt_topics));
+
+     /// CONNECT MQTT CLIENT ///
+    ROS_INFO("Connecting to MQTT...");
+    try {
+        client.set_callback(mqtt2ros);
+        client.connect()->wait();
+        client.subscribe(mqtt_topics.MQTTcommandSub,mqtt_topics.mqttQoS);
+        client.subscribe(mqtt_topics.MQTTcoordSub,mqtt_topics.mqttQoS);
+        client.subscribe(mqtt_topics.MQTTstateSub,mqtt_topics.mqttQoS);
+        client.start_consuming();
+    } catch (mqtt::exception &e) {
+        ROS_ERROR("Failed to Connect to MQTT: [%s]", e.what());
+        ros::shutdown();
+    }
+
+    while (ros::ok()) {
+        ROS_DEBUG_ONCE("Relay Node Looping");
+        ros::spinOnce();
+        loop_rate.sleep();
+    }
+
+    try {
+        client.stop_consuming();
+        client.disable_callbacks();
+        client.disconnect();
+    } catch (mqtt::exception &e) {
+        ROS_DEBUG("Error Disconnecting MQTT: [%s]", e.what());
+    }
+
+    return 0;
+}

src/roboglue_ros/src/roboglue_test.cpp

@@ -0,0 +1,161 @@
+#include <ctime>
+#include <ros/ros.h>
+
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+
+#include <moveit_msgs/DisplayRobotState.h>
+#include <moveit_msgs/DisplayTrajectory.h>
+
+#include <moveit_msgs/AttachedCollisionObject.h>
+#include <moveit_msgs/CollisionObject.h>
+
+#include <moveit_visual_tools/moveit_visual_tools.h>
+#include <Eigen/Eigen>
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "roboglue_test");
+    ros::NodeHandle nh;
+    ros::AsyncSpinner spinner(1);
+    spinner.start();
+    ros::Publisher trajectory_pub = nh.advertise<trajectory_msgs::JointTrajectory>("/pos_based_pos_traj_controller/command", 1000);
+    ROS_INFO("Hello world!");
+
+    //motion group interface
+    static const std::string MOVEG = "manipulator";
+    moveit::planning_interface::MoveGroupInterface movegroup(MOVEG);
+    // to interact with collision world
+    moveit::planning_interface::PlanningSceneInterface scene;
+    // to get informations from planning scene
+    planning_scene_monitor::PlanningSceneMonitor smon("manipulator");
+    smon.startSceneMonitor();
+
+    ROS_INFO("LOADING robot model");
+    //robot kinematic model
+    //robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
+    robot_model::RobotModelConstPtr kine_model = movegroup.getRobotModel();
+    robot_state::RobotStatePtr state_model = movegroup.getCurrentState();
+
+    ROS_INFO("Model base frame: %s", kine_model->getModelFrame().c_str());
+
+    //joint names
+    const robot_state::JointModelGroup* joint_model = movegroup.getCurrentState()->getJointModelGroup(MOVEG);
+    std::vector<std::string> jnames = joint_model->getJointModelNames();
+    for (ulong i=0; i < jnames.size(); i++){
+        ROS_INFO("Joint name[%d]: {%s}", i, jnames[i].c_str());
+    }
+    // We can print the name of the reference frame for this robot.
+    ROS_INFO_NAMED("UR10", "Reference frame: %s", movegroup.getPlanningFrame().c_str());
+    // We can also print the name of the end-effector link for this group.
+    ROS_INFO_NAMED("UR10", "End effector link: %s", movegroup.getEndEffectorLink().c_str());
+
+    namespace rvt = rviz_visual_tools;
+    //visual tools to interact with rviz
+    moveit_visual_tools::MoveItVisualTools vtools("base_link","rviz_visual_tools");
+    //clear visual interface of extraneous objects
+    vtools.loadRemoteControl();
+    vtools.deleteAllMarkers();
+
+    // pose goal
+    moveit::planning_interface::MoveGroupInterface::Plan my_plan_cart;
+    geometry_msgs::PoseStamped currstate_cart = movegroup.getCurrentPose();
+    currstate_cart.pose.position.x = 0;
+    currstate_cart.pose.position.z = 0.25;
+    currstate_cart.pose.position.y = 0.5;
+    movegroup.setPoseTarget(currstate_cart);
+    movegroup.plan(my_plan_cart);
+    vtools.publishText(movegroup.getPoseTarget().pose, "Cartesian Space Goal Position", rvt::RED, rvt::XLARGE);
+    vtools.publishTrajectoryLine(my_plan_cart.trajectory_, joint_model, rvt::colors::RED);
+    vtools.trigger();
+    //move the robot
+    movegroup.move();
+    sleep(2);
+    // control joint position iofference
+    std::vector<double> jpostarget = movegroup.getCurrentJointValues();
+    std::vector<double> jposreal;
+    state_model = movegroup.getCurrentState();
+    state_model->copyJointGroupPositions("manipulator", jposreal);
+    for (ulong i=0; i<jposreal.size(); i++){
+        ROS_INFO("Joint val %d-> t:%+6.3f\tr:%+6.3f",i,jpostarget[i]/M_PI*180,jposreal[i]*180/M_PI);
+    }
+
+    trajectory_msgs::JointTrajectory tr;
+    trajectory_msgs::JointTrajectoryPoint pt;
+    ros::Duration d;
+    d.fromSec(0.0);
+    jnames.pop_back();
+
+    while (ros::ok()){
+        tr.joint_names = jnames;
+        tr.header.frame_id = "/world";
+        tr.header.stamp = ros::Time::now();
+        tr.points.clear();
+
+        pt.positions = {M_PI/2,-M_PI/2,M_PI/2,-M_PI/2,-M_PI/2,0};
+        pt.velocities = {1,1,1,1,1,1};
+        pt.time_from_start = d;
+        d.sec += 2;
+        tr.points.push_back(pt);
+
+        trajectory_pub.publish(tr);
+
+        sleep(20);
+    }
+
+    //catesian path planning
+    geometry_msgs::Pose currpose = movegroup.getCurrentPose().pose;
+    std::vector<geometry_msgs::Pose> wp;
+
+    wp.push_back(currpose);
+    currpose.position.x += 0.25;
+    wp.push_back(currpose);
+    currpose.position.y += 0.5;
+    wp.push_back(currpose);
+    currpose.position.x -= 0.25;
+    currpose.position.z -= 0.25;
+    wp.push_back(currpose);
+    currpose.position.x -= 0.25;
+    currpose.position.z += 0.25;
+    wp.push_back(currpose);
+    currpose.position.y -= 0.5;
+    wp.push_back(currpose);
+
+    const double jmp_thr= 0.0;
+    const double ee_step= 0.01;
+
+    moveit_msgs::RobotTrajectory traj;
+    moveit_msgs::MoveItErrorCodes ec;
+    movegroup.setMaxVelocityScalingFactor(1);
+    movegroup.setPlanningTime(5.0);
+    movegroup.setGoalPositionTolerance(0.005);
+
+    movegroup.computeCartesianPath(wp,ee_step,jmp_thr,traj, false, &ec);
+    ROS_INFO("Trajectory ExitCode: %d",ec.val);
+    traj.joint_trajectory.points.at(1).time_from_start.nsec = 100000;
+
+
+    double h=0.0;
+    for (uint t=0; t<traj.joint_trajectory.points.size(); t++){
+        traj.joint_trajectory.points.at(t).time_from_start.fromSec(h);
+        h+=2.0;
+        ROS_INFO("[%d] Time: %f, Pos: %f %f %f %f %f %f",t, traj.joint_trajectory.points.at(t).time_from_start.toSec(),
+                 traj.joint_trajectory.points.at(t).positions.at(0)*180/M_PI,
+                 traj.joint_trajectory.points.at(t).positions.at(1)*180/M_PI,
+                 traj.joint_trajectory.points.at(t).positions.at(2)*180/M_PI,
+                 traj.joint_trajectory.points.at(t).positions.at(3)*180/M_PI,
+                 traj.joint_trajectory.points.at(t).positions.at(4)*180/M_PI,
+                 traj.joint_trajectory.points.at(t).positions.at(5)*180/M_PI);
+        vtools.publishSphere(wp[t], rvt::BLUE, rvt::MEDIUM);
+    }
+
+    vtools.publishPath(wp, rvt::colors::CYAN, rvt::SMALL);
+    vtools.trigger();
+    my_plan_cart.trajectory_=traj;
+    // move robot
+    ROS_INFO ("Execute trajectory: %d", movegroup.execute(my_plan_cart));
+
+    smon.stopStateMonitor();
+    smon.stopSceneMonitor();
+
+}

src/roboglue_ros/src/roboglue_utils.cpp

@@ -0,0 +1,122 @@
+#include <roboglue_ros/roboglue_utils.h>
+
+/// Utilities funtion used in roboglue ros nodes ///
+
+geometry_msgs::TwistStamped jstr2Twist(nlohmann::json cval){
+    static uint32_t seqId = 0;
+    geometry_msgs::TwistStamped tw;
+    tw.header.stamp = ros::Time::now();
+    tw.header.seq = seqId++;
+    tw.twist.linear.x = cval["x"].get<double>();
+    tw.twist.linear.y = cval["y"].get<double>();
+    tw.twist.linear.z = cval["z"].get<double>();
+    tw.twist.angular.x = cval["rx"].get<double>();
+    tw.twist.angular.y = cval["ry"].get<double>();
+    tw.twist.angular.z = cval["rz"].get<double>();
+    return tw;
+}
+
+trajectory_msgs::JointTrajectory jstr2Joint(nlohmann::json cval){
+    static uint32_t seqId = 0;
+    trajectory_msgs::JointTrajectory jt;
+    trajectory_msgs::JointTrajectoryPoint jtp;
+    jt.header.stamp = ros::Time::now();
+    jt.header.seq = seqId++;
+    jtp.positions = { cval["J1"].get<double>(),
+                    cval["J2"].get<double>(),
+                    cval["J3"].get<double>(),
+                    cval["J4"].get<double>(),
+                    cval["J5"].get<double>(),
+                    cval["J6"].get<double>()
+                    };
+    jt.points.push_back(jtp);
+    return jt;
+}
+
+void getLockInfo(positionData* pos, nlohmann::json lock){
+    pos->lockX = lock["lx"].get<bool>();
+    pos->lockY = lock["ly"].get<bool>();
+    pos->lockZ = lock["lz"].get<bool>();
+    pos->lockRX = lock["lrx"].get<bool>();
+    pos->lockRY = lock["lry"].get<bool>();
+    pos->lockRZ = lock["lrz"].get<bool>();
+}
+
+geometry_msgs::TwistStamped lockTwistPose(geometry_msgs::TwistStamped twtNew, geometry_msgs::TwistStamped twtMem, positionData pos){
+    // lock movement directions
+    if (pos.lockX) twtNew.twist.linear.x = twtMem.twist.linear.x;
+    if (pos.lockY) twtNew.twist.linear.y = twtMem.twist.linear.y;
+    if (pos.lockZ) twtNew.twist.linear.z = twtMem.twist.linear.z;
+    if (pos.lockRX) twtNew.twist.angular.x = twtMem.twist.angular.x;
+    if (pos.lockRY) twtNew.twist.angular.y = twtMem.twist.angular.y;
+    if (pos.lockRZ) twtNew.twist.angular.z = twtMem.twist.angular.z;
+    return twtNew;
+}
+
+geometry_msgs::PoseStamped twist2Pose(geometry_msgs::TwistStamped twt){
+    geometry_msgs::PoseStamped newPoseStamp;
+    tf::Quaternion qt;
+    newPoseStamp.header = twt.header;
+    newPoseStamp.pose.position.y = twt.twist.linear.y;
+    newPoseStamp.pose.position.x = twt.twist.linear.x;
+    newPoseStamp.pose.position.z = twt.twist.linear.z;
+    // FIXME ripristinare il calcolo con le coordinate di posa dal tracker, ora gli assi sono fissi
+    //qt.setRPY(twPose.twist.angular.x, twPose.twist.angular.y, twPose.twist.angular.z);
+    qt.setRPY(0, M_PI/2, M_PI/2);
+    newPoseStamp.pose.orientation.x = qt.x();
+    newPoseStamp.pose.orientation.y = qt.y();
+    newPoseStamp.pose.orientation.z = qt.z();
+    newPoseStamp.pose.orientation.w = qt.w();
+    return newPoseStamp;
+}
+
+double e3dist(geometry_msgs::Twist p1, geometry_msgs::Twist p2){
+    double res;
+    res = pow(p1.linear.x - p2.linear.x, 2.0) +
+            pow(p1.linear.y - p2.linear.y, 2.0) +
+            pow(p1.linear.z - p2.linear.z, 2.0);
+    return fabs(sqrt(res));
+}
+
+double rad2deg(double ang){
+    return ang*180/M_PI;
+}
+
+bool checkJointJump(moveit_msgs::RobotTrajectoryPtr traj, double thresh){
+    // identify max joint jump for each joint
+    typedef struct{
+        double first, second;
+        uint pnum;
+    } jjump;
+    std::vector<jjump> jdist;
+    jdist.resize(traj->joint_trajectory.joint_names.size());
+    double pdist = 0.0;
+    uint pnum=0;
+    bool first = true;
+    for (auto pp : traj->joint_trajectory.points){
+        uint cp = 0;
+        for(auto jpp : pp.positions){
+            if (first) jdist.at(cp).first = jpp;
+            pdist = std::fabs(jdist.at(cp).first - pp.positions.at(cp));
+            if (pdist > jdist.at(cp).second) {
+                jdist.at(cp).second = pdist;
+                jdist.at(cp).pnum = pnum;
+            }
+            jdist.at(cp).first = jpp;
+            cp++;
+        }
+        cp=0;
+        first=false;
+        pnum ++;
+    }
+    uint p = 0;
+    bool rv=false;
+    for (uint jIdx=0; jIdx<jdist.size(); jIdx++){
+        if ((rad2deg(jdist.at(jIdx).second) > thresh)) rv=true;
+    }
+    for (auto jpv : jdist){
+        ROS_WARN_COND(rv,"Joint[%d] max jump = %4.2f at [%d]", 1+p++, rad2deg(jpv.second), jpv.pnum);
+    }
+    return rv;
+}
+