commit 595af0738d18a80b3d8c8bd329214947d28d3d9f Author: emanuele Date: Mon Oct 21 09:57:27 2019 +0200 Primo Commit diff --git a/roboglue_ros_ws.workspace b/roboglue_ros_ws.workspace new file mode 100644 index 0000000..8492138 --- /dev/null +++ b/roboglue_ros_ws.workspace @@ -0,0 +1,8 @@ + + + + + + src + + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt new file mode 120000 index 0000000..581e61d --- /dev/null +++ b/src/CMakeLists.txt @@ -0,0 +1 @@ +/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake \ No newline at end of file diff --git a/src/bio_ik-kinetic-support/CMakeLists.txt b/src/bio_ik-kinetic-support/CMakeLists.txt new file mode 100644 index 0000000..b46c04f --- /dev/null +++ b/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($<$:-O3>) +add_compile_options($<$:-ftree-vectorize>) +add_compile_options($<$:-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}) diff --git a/src/bio_ik-kinetic-support/README.md b/src/bio_ik-kinetic-support/README.md new file mode 100644 index 0000000..480d643 --- /dev/null +++ b/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(), // 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 + + diff --git a/src/bio_ik-kinetic-support/bio_ik_kinematics_description.xml b/src/bio_ik-kinetic-support/bio_ik_kinematics_description.xml new file mode 100644 index 0000000..cf68db3 --- /dev/null +++ b/src/bio_ik-kinetic-support/bio_ik_kinematics_description.xml @@ -0,0 +1,4 @@ + + + + diff --git a/src/bio_ik-kinetic-support/doc/goals.pdf b/src/bio_ik-kinetic-support/doc/goals.pdf new file mode 100644 index 0000000..53fe575 Binary files /dev/null and b/src/bio_ik-kinetic-support/doc/goals.pdf differ diff --git a/src/bio_ik-kinetic-support/doc/pr2_vt_0.png b/src/bio_ik-kinetic-support/doc/pr2_vt_0.png new file mode 100644 index 0000000..f311fdf Binary files /dev/null and b/src/bio_ik-kinetic-support/doc/pr2_vt_0.png differ diff --git a/src/bio_ik-kinetic-support/doc/solvers.pdf b/src/bio_ik-kinetic-support/doc/solvers.pdf new file mode 100644 index 0000000..80f2c65 Binary files /dev/null and b/src/bio_ik-kinetic-support/doc/solvers.pdf differ diff --git a/src/bio_ik-kinetic-support/include/bio_ik/bio_ik.h b/src/bio_ik-kinetic-support/include/bio_ik/bio_ik.h new file mode 100644 index 0000000..6303fd2 --- /dev/null +++ b/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 +#include +#include +#include + +#include + +#include "goal.h" +#include "goal_types.h" + +namespace bio_ik +{ + +} diff --git a/src/bio_ik-kinetic-support/include/bio_ik/frame.h b/src/bio_ik-kinetic-support/include/bio_ik/frame.h new file mode 100644 index 0000000..eadafc4 --- /dev/null +++ b/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 + +#include +#include + +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 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 const Frame Frame::IdentityFrameTemplate::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; +} +} diff --git a/src/bio_ik-kinetic-support/include/bio_ik/goal.h b/src/bio_ik-kinetic-support/include/bio_ik/goal.h new file mode 100644 index 0000000..480ac6d --- /dev/null +++ b/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 + +#include +#include + +namespace bio_ik +{ + +class RobotInfo; + +class GoalContext +{ +protected: + const double* active_variable_positions_; + const Frame* tip_link_frames_; + std::vector goal_variable_indices_; + std::vector goal_link_indices_; + bool goal_secondary_; + std::vector goal_link_names_, goal_variable_names_; + double goal_weight_; + const moveit::core::JointModelGroup* joint_model_group_; + std::vector problem_active_variables_; + std::vector problem_tip_link_indices_; + std::vector initial_guess_; + std::vector velocity_weights_; + const RobotInfo* robot_info_; + mutable std::vector 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& 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> goals; + std::vector fixed_joints; + bool replace; + mutable double solution_fitness; + BioIKKinematicsQueryOptions(); + ~BioIKKinematicsQueryOptions(); +}; +} diff --git a/src/bio_ik-kinetic-support/include/bio_ik/goal_types.h b/src/bio_ik-kinetic-support/include/bio_ik/goal_types.h new file mode 100644 index 0000000..a6957a0 --- /dev/null +++ b/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 + +#include +#include + +#include +#include + +#include +#include + +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 vertices; + std::vector points; + std::vector polygons; + std::vector plane_normals; + std::vector plane_dis; + collision_detection::FCLGeometryConstPtr geometry; + Frame frame; + std::vector> edges; + }; + struct CollisionLink + { + bool initialized; + std::vector> shapes; + CollisionLink() + : initialized(false) + { + } + }; + struct CollisionModel + { + std::vector 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 variable_names; + std::function&)> function; + +public: + JointFunctionGoal() {} + JointFunctionGoal(const std::vector& variable_names, const std::function&)>& function, double weight = 1.0, bool secondary = false) + : variable_names(variable_names) + , function(function) + { + weight_ = weight; + secondary_ = secondary; + } + void setJointVariableNames(const std::vector& n) { variable_names = n; } + void setJointVariableFunction(const std::function&)>& 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 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 function; + +public: + LinkFunctionGoal() {} + LinkFunctionGoal(const std::string& link_name, const std::function& function, double weight = 1.0) + : LinkGoalBase(link_name, weight) + , function(function) + { + } + void setLinkFunction(const std::function& 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; + } +}; +} diff --git a/src/bio_ik-kinetic-support/include/bio_ik/robot_info.h b/src/bio_ik-kinetic-support/include/bio_ik/robot_info.h new file mode 100644 index 0000000..70e6676 --- /dev/null +++ b/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 +#include +#include + +#include + +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 variables; + std::vector activeVariables; + std::vector 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(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; } +}; +} diff --git a/src/bio_ik-kinetic-support/package.xml b/src/bio_ik-kinetic-support/package.xml new file mode 100644 index 0000000..719ca77 --- /dev/null +++ b/src/bio_ik-kinetic-support/package.xml @@ -0,0 +1,47 @@ + + + + bio_ik + 1.0.0 + BioIK for ROS + BSD + + Philipp Sebastian Ruppel + + Philipp Sebastian Ruppel + Michael Goerner + + + catkin + + moveit_core + moveit_core + + pluginlib + pluginlib + + eigen + eigen + + moveit_ros_planning + moveit_ros_planning + + roscpp + roscpp + + tf + tf + + tf_conversions + tf_conversions + + eigen_conversions + eigen_conversions + + rosunit + + + + + + diff --git a/src/bio_ik-kinetic-support/src/forward_kinematics.h b/src/bio_ik-kinetic-support/src/forward_kinematics.h new file mode 100644 index 0000000..904a1f7 --- /dev/null +++ b/src/bio_ik-kinetic-support/src/forward_kinematics.h @@ -0,0 +1,1503 @@ +/********************************************************************* + * 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 +#include +#include +#include + +#if defined(__x86_64__) || defined(__i386__) + +#include +#include +#include + +#if(__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 9)) +#define FUNCTION_MULTIVERSIONING 1 +#else +#define FUNCTION_MULTIVERSIONING 0 +#endif + +#else + +#define FUNCTION_MULTIVERSIONING 0 + +#endif + +namespace bio_ik +{ + +// computes and caches local joint frames +class RobotJointEvaluator +{ +private: + std::vector joint_cache_variables; + std::vector joint_cache_frames; + std::vector link_frames; + +protected: + std::vector joint_axis_list; + moveit::core::RobotModelConstPtr robot_model; + std::vector variables; + +public: + inline void getJointFrame(const moveit::core::JointModel* joint_model, const double* variables, Frame& frame) + { + auto joint_type = joint_model->getType(); + if(joint_type == moveit::core::JointModel::FIXED) + { + frame = Frame::identity(); + return; + } + size_t joint_index = joint_model->getJointIndex(); + switch(joint_type) + { + case moveit::core::JointModel::REVOLUTE: + { + auto axis = joint_axis_list[joint_index]; + + auto v = variables[joint_model->getFirstVariableIndex()]; + + /*v *= 1.0 / (2 * M_PI); + v -= std::floor(v); + v *= (2 * M_PI);*/ + + auto half_angle = v * 0.5; + + // TODO: optimize sin cos + + auto fcos = cos(half_angle); + auto fsin = sin(half_angle); + + // auto fsin = half_angle; + // auto fcos = 1.0 - half_angle * half_angle * 0.5; + + frame = Frame(Vector3(0.0, 0.0, 0.0), Quaternion(axis.x() * fsin, axis.y() * fsin, axis.z() * fsin, fcos)); + + break; + } + case moveit::core::JointModel::PRISMATIC: + { + auto axis = joint_axis_list[joint_index]; + auto v = variables[joint_model->getFirstVariableIndex()]; + frame = Frame(axis * v, Quaternion(0.0, 0.0, 0.0, 1.0)); + break; + } + case moveit::core::JointModel::FLOATING: + { + auto* vv = variables + joint_model->getFirstVariableIndex(); + frame.pos = Vector3(vv[0], vv[1], vv[2]); + frame.rot = Quaternion(vv[3], vv[4], vv[5], vv[6]).normalized(); + // LOG("floating", joint_model->getFirstVariableIndex(), vv[0], vv[1], vv[2], vv[3], vv[4], vv[5], vv[6]); + break; + } + default: + { + auto* joint_variables = variables + joint_model->getFirstVariableIndex(); + Eigen::Affine3d joint_transform; + joint_model->computeTransform(joint_variables, joint_transform); + frame = Frame(joint_transform); + break; + } + } + + // LOG("local joint frame", joint_model->getType(), joint_model->getName(), frame); + } + inline void getJointFrame(const moveit::core::JointModel* joint_model, const std::vector& variables, Frame& frame) { getJointFrame(joint_model, variables.data(), frame); } + +protected: + inline const Frame& getLinkFrame(const moveit::core::LinkModel* link_model) { return link_frames[link_model->getLinkIndex()]; } + + inline bool checkJointMoved(const moveit::core::JointModel* joint_model) + { + size_t i0 = joint_model->getFirstVariableIndex(); + size_t cnt = joint_model->getVariableCount(); + if(cnt == 0) return true; + if(cnt == 1) return !(variables[i0] == joint_cache_variables[i0]); + for(size_t i = i0; i < i0 + cnt; i++) + if(!(variables[i] == joint_cache_variables[i])) return true; + return false; + } + inline void putJointCache(const moveit::core::JointModel* joint_model, const Frame& frame) + { + joint_cache_frames[joint_model->getJointIndex()] = frame; + size_t i0 = joint_model->getFirstVariableIndex(); + size_t cnt = joint_model->getVariableCount(); + for(size_t i = i0; i < i0 + cnt; i++) + joint_cache_variables[i] = variables[i]; + } + inline const Frame& getJointFrame(const moveit::core::JointModel* joint_model) + { + size_t joint_index = joint_model->getJointIndex(); + + if(!checkJointMoved(joint_model)) return joint_cache_frames[joint_index]; + + getJointFrame(joint_model, variables, joint_cache_frames[joint_index]); + + size_t cnt = joint_model->getVariableCount(); + if(cnt) + { + size_t i0 = joint_model->getFirstVariableIndex(); + if(cnt == 1) + joint_cache_variables[i0] = variables[i0]; + else + for(size_t i = i0; i < i0 + cnt; i++) + joint_cache_variables[i] = variables[i]; + } + + // TODO: optimize copy + + /*size_t cnt = joint_model->getVariableCount(); + size_t i0 = joint_model->getFirstVariableIndex(); + memcpy(joint_cache_variables.data() + i0, variables.data() + i0, cnt * 8);*/ + + return joint_cache_frames[joint_index]; + } + +public: + RobotJointEvaluator(moveit::core::RobotModelConstPtr model) + : robot_model(model) + { + joint_cache_variables.clear(); + joint_cache_variables.resize(model->getVariableCount(), DBL_MAX); + + joint_cache_frames.clear(); + joint_cache_frames.resize(model->getJointModelCount()); + + link_frames.clear(); + for(auto* link_model : model->getLinkModels()) + link_frames.push_back(Frame(link_model->getJointOriginTransform())); + + joint_axis_list.clear(); + joint_axis_list.resize(robot_model->getJointModelCount()); + for(size_t i = 0; i < joint_axis_list.size(); i++) + { + auto* joint_model = robot_model->getJointModel(i); + if(auto* j = dynamic_cast(joint_model)) joint_axis_list[i] = Vector3(j->getAxis().x(), j->getAxis().y(), j->getAxis().z()); + if(auto* j = dynamic_cast(joint_model)) joint_axis_list[i] = Vector3(j->getAxis().x(), j->getAxis().y(), j->getAxis().z()); + } + } +}; + +// fast tree fk +class RobotFK_Fast_Base : protected RobotJointEvaluator +{ +protected: + std::vector tip_names; + std::vector tip_frames; + std::vector tip_links; + std::vector link_schedule; + std::vector global_frames; + std::vector> link_chains; + std::vector active_variables; + std::vector> variable_to_link_map; + std::vector variable_to_link_map_2; + std::vector> variable_to_link_chain_map; + inline void updateMimic(std::vector& values) + { + for(auto* joint : robot_model->getMimicJointModels()) + { + auto src = joint->getMimic()->getFirstVariableIndex(); + auto dest = joint->getFirstVariableIndex(); + + // TODO: more dimensions ??? + values[dest] = values[src] * joint->getMimicFactor() + joint->getMimicOffset(); + + // moveit doesn't seem to support multiple mimic dimensions either... ??? + + // TODO: ok??? + // for(size_t base = 0; base < joint->getVariableCount(); base++) + // values[base + dest] = values[base + src] * joint->getMimicFactor() + joint->getMimicOffset(); + } + } + +public: + RobotFK_Fast_Base(moveit::core::RobotModelConstPtr model) + : RobotJointEvaluator(model) + { + } + void initialize(const std::vector& tip_link_indices) + { + + tip_names.resize(tip_link_indices.size()); + for(size_t i = 0; i < tip_link_indices.size(); i++) + tip_names[i] = robot_model->getLinkModelNames()[tip_link_indices[i]]; + + tip_frames.resize(tip_names.size()); + + tip_links.clear(); + for(const auto& n : tip_names) + tip_links.push_back(robot_model->getLinkModel(n)); + + global_frames.resize(robot_model->getLinkModelCount()); + + link_chains.clear(); + link_schedule.clear(); + for(auto* tip_link : tip_links) + { + std::vector chain; + for(auto* link = tip_link; link; link = link->getParentLinkModel()) + chain.push_back(link); + reverse(chain.begin(), chain.end()); + link_chains.push_back(chain); + for(auto* link : chain) + { + if(find(link_schedule.begin(), link_schedule.end(), link) != link_schedule.end()) continue; + link_schedule.push_back(link); + } + } + + active_variables.clear(); + for(auto* link_model : link_schedule) + { + auto link_index = link_model->getLinkIndex(); + auto* joint_model = link_model->getParentJointModel(); + size_t vstart = joint_model->getFirstVariableIndex(); + size_t vcount = joint_model->getVariableCount(); + for(size_t vi = vstart; vi < vstart + vcount; vi++) + { + variable_to_link_map.push_back(std::make_pair(vi, link_index)); + + if(find(active_variables.begin(), active_variables.end(), vi) != active_variables.end()) continue; + active_variables.push_back(vi); + } + } + + variable_to_link_map_2.resize(robot_model->getVariableCount()); + for(auto* link_model : link_schedule) + { + auto link_index = link_model->getLinkIndex(); + auto* joint_model = link_model->getParentJointModel(); + size_t vstart = joint_model->getFirstVariableIndex(); + size_t vcount = joint_model->getVariableCount(); + for(size_t vi = vstart; vi < vstart + vcount; vi++) + { + variable_to_link_map_2[vi] = link_index; + } + } + + variable_to_link_chain_map.resize(robot_model->getVariableCount()); + for(size_t chain_index = 0; chain_index < link_chains.size(); chain_index++) + { + auto& link_chain = link_chains[chain_index]; + for(size_t ipos = 0; ipos < link_chain.size(); ipos++) + { + auto* link_model = link_chain[ipos]; + auto* joint_model = link_model->getParentJointModel(); + size_t vstart = joint_model->getFirstVariableIndex(); + size_t vcount = joint_model->getVariableCount(); + for(size_t vi = vstart; vi < vstart + vcount; vi++) + { + variable_to_link_chain_map[vi] = std::make_pair(chain_index, ipos); + } + } + } + } + void applyConfiguration(const std::vector& jj0) + { + FNPROFILER(); + variables = jj0; + updateMimic(variables); + for(auto* link_model : link_schedule) + { + auto* joint_model = link_model->getParentJointModel(); + auto* parent_link_model = joint_model->getParentLinkModel(); + if(parent_link_model) + { + concat(global_frames[parent_link_model->getLinkIndex()], getLinkFrame(link_model), getJointFrame(joint_model), global_frames[link_model->getLinkIndex()]); + } + else + { + concat(getLinkFrame(link_model), getJointFrame(joint_model), global_frames[link_model->getLinkIndex()]); + } + // if(linkModel->getParentLinkModel() && linkModel->getParentLinkModel()->getLinkIndex() > linkModel->getLinkIndex()) { LOG("wrong link order"); throw runtime_error("wrong link order"); } + } + for(size_t itip = 0; itip < tip_links.size(); itip++) + { + tip_frames[itip] = global_frames[tip_links[itip]->getLinkIndex()]; + } + } + inline const Frame& getTipFrame(size_t fi) const { return tip_frames[fi]; } + inline const std::vector& getTipFrames() const { return tip_frames; } + inline void incrementalBegin(const std::vector& jj) { applyConfiguration(jj); } + inline void incrementalEnd() {} + inline const Frame& getJointVariableFrame(size_t variable_index) { return global_frames[variable_to_link_map_2[variable_index]]; } +}; + +// incremental tip frame updates without full recomputation +class RobotFK_Fast : public RobotFK_Fast_Base +{ +protected: + std::vector> chain_cache; + std::vector vars, variables0; + std::vector test_tips; + std::vector links_changed; + bool use_incremental; + bool last_use_incremental; + void updateIncremental(const std::vector& vars0) + { + FNPROFILER(); + + // PROFILER_COUNTER("incremental update count"); + + // init variable lists + vars = vars0; + updateMimic(vars); + for(auto& vi : active_variables) + { + // vars[vi] = vars0[vi]; + variables0[vi] = variables[vi]; + } + + // find moved links + for(auto* link_model : link_schedule) + links_changed[link_model->getLinkIndex()] = false; + for(auto& x : variable_to_link_map) + { + auto variable_index = x.first; + auto link_index = x.second; + if(vars[variable_index] != variables[variable_index]) links_changed[link_index] = true; + } + + // update + for(size_t ichain = 0; ichain < link_chains.size(); ichain++) + { + auto& link_chain = link_chains[ichain]; + auto& cache_chain = chain_cache[ichain]; + + size_t iposmax = 0; + for(size_t ipos = 0; ipos < link_chain.size(); ipos++) + { + auto* link_model = link_chain[ipos]; + bool changed = links_changed[link_model->getLinkIndex()]; + if(changed) iposmax = ipos; + } + + for(size_t ipos = 0; ipos <= iposmax; ipos++) + { + auto* link_model = link_chain[ipos]; + auto* joint_model = link_model->getParentJointModel(); + + bool changed = links_changed[link_model->getLinkIndex()]; + + if(cache_chain.size() <= ipos || changed) + { + Frame before, after; + + if(ipos < cache_chain.size()) + { + before = cache_chain[ipos]; + } + else + { + if(ipos > 0) + { + concat(cache_chain[ipos - 1], getLinkFrame(link_model), getJointFrame(joint_model), before); + } + else + { + concat(getLinkFrame(link_model), getJointFrame(joint_model), before); + } + } + + chain_cache[ichain].resize(ipos + 1, Frame::identity()); + + if(changed) + { + if(ichain > 0 && ipos < link_chains[ichain - 1].size() && link_chains[ichain][ipos] == link_chains[ichain - 1][ipos]) + { + after = chain_cache[ichain - 1][ipos]; + } + else + { + size_t vstart = joint_model->getFirstVariableIndex(); + size_t vcount = joint_model->getVariableCount(); + + if(vcount == 1) + variables[vstart] = vars[vstart]; + else + for(size_t vi = vstart; vi < vstart + vcount; vi++) + variables[vi] = vars[vi]; + + if(ipos > 0) + { + concat(cache_chain[ipos - 1], getLinkFrame(link_model), getJointFrame(joint_model), after); + } + else + { + concat(getLinkFrame(link_model), getJointFrame(joint_model), after); + } + + if(vcount == 1) + variables[vstart] = variables0[vstart]; + else + for(size_t vi = vstart; vi < vstart + vcount; vi++) + variables[vi] = variables0[vi]; + + // PROFILER_COUNTER("incremental update transforms"); + } + chain_cache[ichain][ipos] = after; + + // tipFrames[ichain] = inverse(before) * tipFrames[ichain]; + // tipFrames[ichain] = after * tipFrames[ichain]; + + /*Frame before_inverse; + invert(before, before_inverse); + + //tipFrames[ichain] = after * before_inverse * tipFrames[ichain]; + { + concat(after, before_inverse, tip_frames[ichain], tip_frames[ichain]); + }*/ + + change(after, before, tip_frames[ichain], tip_frames[ichain]); + } + else + { + after = before; + chain_cache[ichain][ipos] = after; + } + } + } + } + + // set new vars + // variables = vars; + for(auto& vi : active_variables) + variables[vi] = vars[vi]; + + // test + if(0) + { + test_tips = tip_frames; + RobotFK_Fast_Base::applyConfiguration(vars); + for(size_t i = 0; i < tip_frames.size(); i++) + { + auto dist = tip_frames[i].pos.distance(test_tips[i].pos); + LOG_VAR(dist); + if(dist > 0.001) + { + LOG(tip_frames[i].pos.x(), tip_frames[i].pos.y(), tip_frames[i].pos.z()); + LOG(test_tips[i].pos.x(), test_tips[i].pos.y(), test_tips[i].pos.z()); + ERROR("incremental update error"); + } + } + } + } + +public: + RobotFK_Fast(moveit::core::RobotModelConstPtr model) + : RobotFK_Fast_Base(model) + , use_incremental(false) + { + } + inline void incrementalBegin(const std::vector& jj) + { + applyConfiguration(jj); + chain_cache.clear(); + chain_cache.resize(tip_frames.size()); + links_changed.resize(robot_model->getLinkModelCount()); + vars.resize(jj.size()); + variables0.resize(jj.size()); + use_incremental = true; + } + inline void incrementalEnd() + { + use_incremental = false; + // applyConfiguration(variables); + } + inline void applyConfiguration(const std::vector& jj) + { + if(use_incremental) + updateIncremental(jj); + else + RobotFK_Fast_Base::applyConfiguration(jj); + } +}; + +// fast tree fk jacobian +class RobotFK_Jacobian : public RobotFK_Fast +{ + typedef RobotFK_Fast Base; + +protected: + std::vector> joint_dependencies; + std::vector tip_dependencies; + +public: + RobotFK_Jacobian(moveit::core::RobotModelConstPtr model) + : RobotFK_Fast(model) + { + } + void initialize(const std::vector& tip_link_indices) + { + Base::initialize(tip_link_indices); + auto tip_count = tip_names.size(); + joint_dependencies.resize(robot_model->getJointModelCount()); + for(auto& x : joint_dependencies) + x.clear(); + for(auto* link_model : link_schedule) + { + auto* joint_model = link_model->getParentJointModel(); + joint_dependencies[joint_model->getJointIndex()].push_back(joint_model); + } + for(auto* link_model : link_schedule) + { + auto* joint_model = link_model->getParentJointModel(); + if(auto* mimic = joint_model->getMimic()) + { + while(mimic->getMimic() && mimic->getMimic() != joint_model) + mimic = mimic->getMimic(); + joint_dependencies[mimic->getJointIndex()].push_back(joint_model); + } + } + tip_dependencies.resize(robot_model->getJointModelCount() * tip_count); + for(auto& x : tip_dependencies) + x = 0; + for(size_t tip_index = 0; tip_index < tip_count; tip_index++) + { + for(auto* link_model = tip_links[tip_index]; link_model; link_model = link_model->getParentLinkModel()) + { + auto* joint_model = link_model->getParentJointModel(); + tip_dependencies[joint_model->getJointIndex() * tip_count + tip_index] = 1; + } + } + } + void computeJacobian(const std::vector& variable_indices, Eigen::MatrixXd& jacobian) + { + double step_size = 0.00001; + double half_step_size = step_size * 0.5; + double inv_step_size = 1.0 / step_size; + auto tip_count = tip_frames.size(); + jacobian.resize(tip_count * 6, variable_indices.size()); + for(size_t icol = 0; icol < variable_indices.size(); icol++) + { + for(size_t itip = 0; itip < tip_count; itip++) + { + jacobian(itip * 6 + 0, icol) = 0; + jacobian(itip * 6 + 1, icol) = 0; + jacobian(itip * 6 + 2, icol) = 0; + jacobian(itip * 6 + 3, icol) = 0; + jacobian(itip * 6 + 4, icol) = 0; + jacobian(itip * 6 + 5, icol) = 0; + } + } + for(size_t icol = 0; icol < variable_indices.size(); icol++) + { + auto ivar = variable_indices[icol]; + auto* var_joint_model = robot_model->getJointOfVariable(ivar); + if(var_joint_model->getMimic()) continue; + for(auto* joint_model : joint_dependencies[var_joint_model->getJointIndex()]) + { + double scale = 1; + for(auto* m = joint_model; m->getMimic() && m->getMimic() != joint_model; m = m->getMimic()) + { + scale *= m->getMimicFactor(); + } + auto* link_model = joint_model->getChildLinkModel(); + switch(joint_model->getType()) + { + case moveit::core::JointModel::FIXED: + { + // fixed joint: zero gradient (do nothing) + continue; + } + case moveit::core::JointModel::REVOLUTE: + { + auto& link_frame = global_frames[link_model->getLinkIndex()]; + for(size_t itip = 0; itip < tip_count; itip++) + { + if(!tip_dependencies[joint_model->getJointIndex() * tip_count + itip]) continue; + + auto& tip_frame = tip_frames[itip]; + + auto q = link_frame.rot.inverse() * tip_frame.rot; + q = q.inverse(); + + auto rot = joint_axis_list[joint_model->getJointIndex()]; + quat_mul_vec(q, rot, rot); + + auto vel = link_frame.pos - tip_frame.pos; + quat_mul_vec(tip_frame.rot.inverse(), vel, vel); + + vel = vel.cross(rot); + + jacobian(itip * 6 + 0, icol) += vel.x() * scale; + jacobian(itip * 6 + 1, icol) += vel.y() * scale; + jacobian(itip * 6 + 2, icol) += vel.z() * scale; + + jacobian(itip * 6 + 3, icol) += rot.x() * scale; + jacobian(itip * 6 + 4, icol) += rot.y() * scale; + jacobian(itip * 6 + 5, icol) += rot.z() * scale; + + // LOG("a", vel.x(), vel.y(), vel.z(), rot.x(), rot.y(), rot.z()); + } + continue; + } + case moveit::core::JointModel::PRISMATIC: + { + auto& link_frame = global_frames[link_model->getLinkIndex()]; + for(size_t itip = 0; itip < tip_count; itip++) + { + if(!tip_dependencies[joint_model->getJointIndex() * tip_count + itip]) continue; + + auto& tip_frame = tip_frames[itip]; + + auto q = link_frame.rot.inverse() * tip_frame.rot; + q = q.inverse(); + + auto axis = joint_axis_list[joint_model->getJointIndex()]; + auto v = axis; + quat_mul_vec(q, axis, v); + + jacobian(itip * 6 + 0, icol) += v.x() * scale; + jacobian(itip * 6 + 1, icol) += v.y() * scale; + jacobian(itip * 6 + 2, icol) += v.z() * scale; + + // LOG("a", v.x(), v.y(), v.z(), 0, 0, 0); + } + continue; + } + default: + { + // numeric differentiation for joint types that are not yet implemented / or joint types that might be added to moveit in the future + auto ivar2 = ivar; + if(joint_model->getMimic()) ivar2 = ivar2 - var_joint_model->getFirstVariableIndex() + joint_model->getFirstVariableIndex(); + auto& link_frame_1 = global_frames[link_model->getLinkIndex()]; + auto v0 = variables[ivar2]; + // auto joint_frame_1 = getJointFrame(joint_model); + variables[ivar2] = v0 + step_size; + auto joint_frame_2 = getJointFrame(joint_model); + variables[ivar2] = v0; + Frame link_frame_2; + if(auto* parent_link_model = joint_model->getParentLinkModel()) + concat(global_frames[parent_link_model->getLinkIndex()], getLinkFrame(link_model), joint_frame_2, link_frame_2); + else + concat(getLinkFrame(link_model), joint_frame_2, link_frame_2); + for(size_t itip = 0; itip < tip_count; itip++) + { + if(!tip_dependencies[joint_model->getJointIndex() * tip_count + itip]) continue; + auto tip_frame_1 = tip_frames[itip]; + Frame tip_frame_2; + change(link_frame_2, link_frame_1, tip_frame_1, tip_frame_2); + auto twist = frameTwist(tip_frame_1, tip_frame_2); + jacobian(itip * 6 + 0, icol) += twist.vel.x() * inv_step_size * scale; + jacobian(itip * 6 + 1, icol) += twist.vel.y() * inv_step_size * scale; + jacobian(itip * 6 + 2, icol) += twist.vel.z() * inv_step_size * scale; + jacobian(itip * 6 + 3, icol) += twist.rot.x() * inv_step_size * scale; + jacobian(itip * 6 + 4, icol) += twist.rot.y() * inv_step_size * scale; + jacobian(itip * 6 + 5, icol) += twist.rot.z() * inv_step_size * scale; + } + continue; + } + } + } + } + } +}; + +#if 0 + +struct RobotFK : public RobotFK_Jacobian +{ + std::vector mutation_variable_indices; + std::vector mutation_initial_variable_positions; + std::vector mutation_temp; + RobotFK(moveit::core::RobotModelConstPtr model) + : RobotFK_Jacobian(model) + { + } + void initializeMutationApproximator(const std::vector& variable_indices) + { + mutation_variable_indices = variable_indices; + mutation_initial_variable_positions = variables; + } + void computeApproximateMutation1(size_t variable_index, double variable_delta, const aligned_vector& input, aligned_vector& output) + { + mutation_temp = mutation_initial_variable_positions; + mutation_temp[variable_index] += variable_delta; + applyConfiguration(mutation_temp); + output.clear(); + for(auto& f : tip_frames) output.push_back(f); + } + void computeApproximateMutations(size_t mutation_count, const double* const* mutation_values, std::vector>& tip_frame_mutations) + { + auto tip_count = tip_names.size(); + tip_frame_mutations.resize(mutation_count); + for(auto& m : tip_frame_mutations) + m.resize(tip_count); + for(size_t mutation_index = 0; mutation_index < mutation_count; mutation_index++) + { + mutation_temp = mutation_initial_variable_positions; + for(size_t i = 0; i < mutation_variable_indices.size(); i++) + { + mutation_temp[mutation_variable_indices[i]] = mutation_values[mutation_index][i]; + } + applyConfiguration(mutation_temp); + for(size_t tip_index = 0; tip_index < tip_count; tip_index++) + { + tip_frame_mutations[mutation_index][tip_index] = tip_frames[tip_index]; + } + } + } +}; + +#else + +#define USE_QUADRATIC_EXTRAPOLATION 0 + +class RobotFK_Mutator : public RobotFK_Jacobian +{ + typedef RobotFK_Jacobian Base; + Eigen::MatrixXd mutation_approx_jacobian; + std::vector> mutation_approx_frames; +#if USE_QUADRATIC_EXTRAPOLATION + std::vector> mutation_approx_quadratics; +#endif + std::vector mutation_approx_variable_indices; + std::vector> mutation_approx_mask; + std::vector> mutation_approx_map; + aligned_vector tip_frames_aligned; + +public: + RobotFK_Mutator(moveit::core::RobotModelConstPtr model) + : RobotFK_Jacobian(model) + { + } + + void initializeMutationApproximator(const std::vector& variable_indices) + { + FNPROFILER(); + + mutation_approx_variable_indices = variable_indices; + + auto tip_count = tip_names.size(); + + tip_frames_aligned.resize(tip_frames.size()); + for(size_t i = 0; i < tip_frames.size(); i++) + tip_frames_aligned[i] = tip_frames[i]; + + // init first order approximators + { + if(mutation_approx_frames.size() < tip_count) mutation_approx_frames.resize(tip_count); + + for(size_t itip = 0; itip < tip_count; itip++) + mutation_approx_frames[itip].resize(robot_model->getVariableCount()); + + for(size_t itip = 0; itip < tip_count; itip++) + for(auto ivar : variable_indices) + mutation_approx_frames[itip][ivar] = Frame::identity(); + + computeJacobian(variable_indices, mutation_approx_jacobian); + + for(size_t icol = 0; icol < variable_indices.size(); icol++) + { + size_t ivar = variable_indices[icol]; + for(size_t itip = 0; itip < tip_count; itip++) + { + { + Vector3 t; + t.setX(mutation_approx_jacobian(itip * 6 + 0, icol)); + t.setY(mutation_approx_jacobian(itip * 6 + 1, icol)); + t.setZ(mutation_approx_jacobian(itip * 6 + 2, icol)); + quat_mul_vec(tip_frames[itip].rot, t, t); + mutation_approx_frames[itip][ivar].pos = t; + } + + { + Quaternion q; + q.setX(mutation_approx_jacobian(itip * 6 + 3, icol) * 0.5); + q.setY(mutation_approx_jacobian(itip * 6 + 4, icol) * 0.5); + q.setZ(mutation_approx_jacobian(itip * 6 + 5, icol) * 0.5); + q.setW(1.0); + quat_mul_quat(tip_frames[itip].rot, q, q); + q -= tip_frames[itip].rot; + mutation_approx_frames[itip][ivar].rot = q; + } + } + } + } + +// init second order approximators +#if USE_QUADRATIC_EXTRAPOLATION + { + if(mutation_approx_quadratics.size() < tip_count) mutation_approx_quadratics.resize(tip_count); + for(size_t itip = 0; itip < tip_count; itip++) + mutation_approx_quadratics[itip].resize(robot_model->getVariableCount()); + for(size_t itip = 0; itip < tip_count; itip++) + for(auto ivar : variable_indices) + mutation_approx_quadratics[itip][ivar].pos = Vector3(0, 0, 0); + for(auto ivar : variable_indices) + { + auto* var_joint_model = robot_model->getJointOfVariable(ivar); + if(var_joint_model->getMimic()) continue; + for(auto* joint_model : joint_dependencies[var_joint_model->getJointIndex()]) + { + double scale = 1; + for(auto* m = joint_model; m->getMimic() && m->getMimic() != joint_model; m = m->getMimic()) + { + scale *= m->getMimicFactor(); + } + auto* link_model = joint_model->getChildLinkModel(); + switch(joint_model->getType()) + { + case moveit::core::JointModel::REVOLUTE: + { + auto& link_frame = global_frames[link_model->getLinkIndex()]; + for(size_t itip = 0; itip < tip_count; itip++) + { + if(!tip_dependencies[joint_model->getJointIndex() * tip_count + itip]) continue; + + auto& tip_frame = tip_frames[itip]; + + auto axis = joint_axis_list[joint_model->getJointIndex()]; + quat_mul_vec(link_frame.rot, axis, axis); + + auto v = link_frame.pos - tip_frame.pos; + v -= axis * v.dot(axis); + + mutation_approx_quadratics[itip][ivar].pos = v * 0.5 * (scale * scale); + } + continue; + } + default: + { + continue; + } + } + } + } + } +#endif + + // init mask + if(mutation_approx_mask.size() < tip_count) mutation_approx_mask.resize(tip_count); + if(mutation_approx_map.size() < tip_count) mutation_approx_map.resize(tip_count); + for(size_t itip = 0; itip < tip_count; itip++) + { + if(mutation_approx_mask[itip].size() < robot_model->getVariableCount()) mutation_approx_mask[itip].resize(robot_model->getVariableCount()); + mutation_approx_map[itip].clear(); + for(size_t ii = 0; ii < variable_indices.size(); ii++) + // for(size_t ivar : variable_indices) + { + auto ivar = variable_indices[ii]; + auto& frame = mutation_approx_frames[itip][ivar]; + bool b = false; + b |= (frame.pos.x() != 0.0); + b |= (frame.pos.y() != 0.0); + b |= (frame.pos.z() != 0.0); + b |= (frame.rot.x() != 0.0); + b |= (frame.rot.y() != 0.0); + b |= (frame.rot.z() != 0.0); + mutation_approx_mask[itip][ivar] = b; + if(b) mutation_approx_map[itip].push_back(ii); + } + } + } + +#if FUNCTION_MULTIVERSIONING + __attribute__((hot)) __attribute__((noinline)) __attribute__((target("fma", "avx"))) void computeApproximateMutation1(size_t variable_index, double variable_delta, const aligned_vector& input, aligned_vector& output) const + { + // BLOCKPROFILER("computeApproximateMutations C"); + auto tip_count = tip_names.size(); + output.resize(tip_count); + for(size_t itip = 0; itip < tip_count; itip++) + { + if(mutation_approx_mask[itip][variable_index] == 0) continue; + auto& joint_delta = mutation_approx_frames[itip][variable_index]; + const Frame& tip_frame = input[itip]; + const double* tip_frame_ptr = (const double*)&tip_frame; + __m256d p = _mm256_load_pd(tip_frame_ptr + 0); + __m256d r = _mm256_load_pd(tip_frame_ptr + 4); + { + auto joint_delta_ptr = (const double* __restrict__) & (joint_delta); + __m256d ff = _mm256_set1_pd(variable_delta); + p = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 0), p); + r = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 4), r); + } +#if USE_QUADRATIC_EXTRAPOLATION + { + auto joint_delta_ptr = (const double* __restrict__) & (mutation_approx_quadratics[itip][variable_index]); + __m256d ff = _mm256_set1_pd(variable_delta * variable_delta); + p = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 0), p); + } +#endif + auto& tip_mutation = output[itip]; + double* __restrict__ tip_mutation_ptr = (double*)&tip_mutation; + _mm256_store_pd(tip_mutation_ptr + 0, p); + _mm256_store_pd(tip_mutation_ptr + 4, r); + } + } + + __attribute__((hot)) __attribute__((noinline)) __attribute__((target("sse2"))) void computeApproximateMutation1(size_t variable_index, double variable_delta, const aligned_vector& input, aligned_vector& output) const + { + // BLOCKPROFILER("computeApproximateMutations C"); + auto tip_count = tip_names.size(); + output.resize(tip_count); + for(size_t itip = 0; itip < tip_count; itip++) + { + if(mutation_approx_mask[itip][variable_index] == 0) continue; + auto& joint_delta = mutation_approx_frames[itip][variable_index]; + const Frame& tip_frame = input[itip]; + const double* tip_frame_ptr = (const double*)&tip_frame; + __m128d pxy = _mm_load_pd(tip_frame_ptr + 0); + __m128d pzw = _mm_load_sd(tip_frame_ptr + 2); + __m128d rxy = _mm_load_pd(tip_frame_ptr + 4); + __m128d rzw = _mm_load_pd(tip_frame_ptr + 6); + { + auto joint_delta_ptr = (const double* __restrict__) & (joint_delta); + __m128d ff = _mm_set1_pd(variable_delta); + pxy = _mm_add_pd(pxy, _mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 0))); + pzw = _mm_add_sd(pzw, _mm_mul_sd(ff, _mm_load_sd(joint_delta_ptr + 2))); + rxy = _mm_add_pd(rxy, _mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 4))); + rzw = _mm_add_pd(rzw, _mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 6))); + } +#if USE_QUADRATIC_EXTRAPOLATION + { + auto joint_delta_ptr = (const double* __restrict__) & (mutation_approx_quadratics[itip][variable_index]); + __m128d ff = _mm_set1_pd(variable_delta * variable_delta); + pxy = _mm_add_pd(pxy, _mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 0))); + pzw = _mm_add_sd(pzw, _mm_mul_sd(ff, _mm_load_sd(joint_delta_ptr + 2))); + } +#endif + auto& tip_mutation = output[itip]; + double* __restrict__ tip_mutation_ptr = (double*)&tip_mutation; + _mm_store_pd(tip_mutation_ptr + 0, pxy); + _mm_store_sd(tip_mutation_ptr + 2, pzw); + _mm_store_pd(tip_mutation_ptr + 4, rxy); + _mm_store_pd(tip_mutation_ptr + 6, rzw); + } + } + + __attribute__((hot)) __attribute__((noinline)) __attribute__((target("default"))) +#endif + void + computeApproximateMutation1(size_t variable_index, double variable_delta, const aligned_vector& input, aligned_vector& output) const + { + // BLOCKPROFILER("computeApproximateMutations C"); + auto tip_count = tip_names.size(); + output.resize(tip_count); + for(size_t itip = 0; itip < tip_count; itip++) + { + if(mutation_approx_mask[itip][variable_index] == 0) continue; + + auto& joint_delta = mutation_approx_frames[itip][variable_index]; + + const Frame& tip_frame = input[itip]; + + double px = tip_frame.pos.x(); + double py = tip_frame.pos.y(); + double pz = tip_frame.pos.z(); + + double rx = tip_frame.rot.x(); + double ry = tip_frame.rot.y(); + double rz = tip_frame.rot.z(); + double rw = tip_frame.rot.w(); + + px += joint_delta.pos.x() * variable_delta; + py += joint_delta.pos.y() * variable_delta; + pz += joint_delta.pos.z() * variable_delta; + +#if USE_QUADRATIC_EXTRAPOLATION + double variable_delta_sq = variable_delta * variable_delta; + px += mutation_approx_quadratics[itip][variable_index].pos.x() * variable_delta_sq; + py += mutation_approx_quadratics[itip][variable_index].pos.y() * variable_delta_sq; + pz += mutation_approx_quadratics[itip][variable_index].pos.z() * variable_delta_sq; +#endif + + rx += joint_delta.rot.x() * variable_delta; + ry += joint_delta.rot.y() * variable_delta; + rz += joint_delta.rot.z() * variable_delta; + rw += joint_delta.rot.w() * variable_delta; + + auto& tip_mutation = output[itip]; + + tip_mutation.pos.setX(px); + tip_mutation.pos.setY(py); + tip_mutation.pos.setZ(pz); + + tip_mutation.rot.setX(rx); + tip_mutation.rot.setY(ry); + tip_mutation.rot.setZ(rz); + tip_mutation.rot.setW(rw); + } + } + +#if FUNCTION_MULTIVERSIONING + __attribute__((hot)) __attribute__((noinline)) __attribute__((target("fma", "avx"))) void computeApproximateMutations(size_t mutation_count, const double* const* mutation_values, std::vector>& tip_frame_mutations) const + { + const double* p_variables = variables.data(); + auto tip_count = tip_names.size(); + tip_frame_mutations.resize(mutation_count); + for(auto& m : tip_frame_mutations) + m.resize(tip_count); + for(size_t itip = 0; itip < tip_count; itip++) + { + auto& joint_deltas = mutation_approx_frames[itip]; + + const Frame& tip_frame = tip_frames_aligned[itip]; + + const double* tip_frame_ptr = (const double*)&tip_frame; + __m256d p0 = _mm256_load_pd(tip_frame_ptr + 0); + __m256d r0 = _mm256_load_pd(tip_frame_ptr + 4); + + for(size_t imutation = 0; imutation < mutation_count; imutation++) + { + auto p = p0; + auto r = r0; + + for(size_t vii : mutation_approx_map[itip]) + { + size_t variable_index = mutation_approx_variable_indices[vii]; + double variable_delta = mutation_values[imutation][vii] - p_variables[variable_index]; + + { + auto joint_delta_ptr = (const double* __restrict__) & (joint_deltas[variable_index]); + __m256d ff = _mm256_set1_pd(variable_delta); + p = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 0), p); + r = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 4), r); + } + +#if USE_QUADRATIC_EXTRAPOLATION + { + auto joint_delta_ptr = (const double* __restrict__) & (mutation_approx_quadratics[itip][variable_index]); + __m256d ff = _mm256_set1_pd(variable_delta * variable_delta); + p = _mm256_fmadd_pd(ff, _mm256_load_pd(joint_delta_ptr + 0), p); + } +#endif + } + + auto& tip_mutation = tip_frame_mutations[imutation][itip]; + double* __restrict__ tip_mutation_ptr = (double*)&tip_mutation; + _mm256_store_pd(tip_mutation_ptr + 0, p); + _mm256_store_pd(tip_mutation_ptr + 4, r); + } + } + } + + __attribute__((hot)) __attribute__((noinline)) __attribute__((target("sse2"))) void computeApproximateMutations(size_t mutation_count, const double* const* mutation_values, std::vector>& tip_frame_mutations) const + { + const double* p_variables = variables.data(); + auto tip_count = tip_names.size(); + tip_frame_mutations.resize(mutation_count); + for(auto& m : tip_frame_mutations) + m.resize(tip_count); + for(size_t itip = 0; itip < tip_count; itip++) + { + auto& joint_deltas = mutation_approx_frames[itip]; + + const Frame& tip_frame = tip_frames_aligned[itip]; + + const double* tip_frame_ptr = (const double*)&tip_frame; + __m128d pxy0 = _mm_load_pd(tip_frame_ptr + 0); + __m128d pzw0 = _mm_load_sd(tip_frame_ptr + 2); + __m128d rxy0 = _mm_load_pd(tip_frame_ptr + 4); + __m128d rzw0 = _mm_load_pd(tip_frame_ptr + 6); + + for(size_t imutation = 0; imutation < mutation_count; imutation++) + { + auto pxy = pxy0; + auto pzw = pzw0; + auto rxy = rxy0; + auto rzw = rzw0; + + for(size_t vii : mutation_approx_map[itip]) + { + size_t variable_index = mutation_approx_variable_indices[vii]; + double variable_delta = mutation_values[imutation][vii] - p_variables[variable_index]; + + { + auto joint_delta_ptr = (const double* __restrict__) & (joint_deltas[variable_index]); + __m128d ff = _mm_set1_pd(variable_delta); + pxy = _mm_add_pd(_mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 0)), pxy); + pzw = _mm_add_sd(_mm_mul_sd(ff, _mm_load_sd(joint_delta_ptr + 2)), pzw); + rxy = _mm_add_pd(_mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 4)), rxy); + rzw = _mm_add_pd(_mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 6)), rzw); + } + +#if USE_QUADRATIC_EXTRAPOLATION + { + auto joint_delta_ptr = (const double* __restrict__) & (mutation_approx_quadratics[itip][variable_index]); + __m128d ff = _mm_set1_pd(variable_delta * variable_delta); + pxy = _mm_add_pd(_mm_mul_pd(ff, _mm_load_pd(joint_delta_ptr + 0)), pxy); + pzw = _mm_add_sd(_mm_mul_sd(ff, _mm_load_sd(joint_delta_ptr + 2)), pzw); + } +#endif + } + + auto& tip_mutation = tip_frame_mutations[imutation][itip]; + double* __restrict__ tip_mutation_ptr = (double*)&tip_mutation; + _mm_store_pd(tip_mutation_ptr + 0, pxy); + _mm_store_sd(tip_mutation_ptr + 2, pzw); + _mm_store_pd(tip_mutation_ptr + 4, rxy); + _mm_store_pd(tip_mutation_ptr + 6, rzw); + } + } + } + + __attribute__((hot)) __attribute__((noinline)) __attribute__((target("default"))) +#endif + void + computeApproximateMutations(size_t mutation_count, const double* const* mutation_values, std::vector>& tip_frame_mutations) const + { + const double* p_variables = variables.data(); + auto tip_count = tip_names.size(); + tip_frame_mutations.resize(mutation_count); + for(auto& m : tip_frame_mutations) + m.resize(tip_count); + for(size_t itip = 0; itip < tip_count; itip++) + { + auto& joint_deltas = mutation_approx_frames[itip]; + + const Frame& tip_frame = tip_frames[itip]; + for(size_t imutation = 0; imutation < mutation_count; imutation++) + { + double px = tip_frame.pos.x(); + double py = tip_frame.pos.y(); + double pz = tip_frame.pos.z(); + + double rx = tip_frame.rot.x(); + double ry = tip_frame.rot.y(); + double rz = tip_frame.rot.z(); + double rw = tip_frame.rot.w(); + + for(size_t vii : mutation_approx_map[itip]) + { + size_t variable_index = mutation_approx_variable_indices[vii]; + + double variable_delta = mutation_values[imutation][vii] - p_variables[variable_index]; + + px += joint_deltas[variable_index].pos.x() * variable_delta; + py += joint_deltas[variable_index].pos.y() * variable_delta; + pz += joint_deltas[variable_index].pos.z() * variable_delta; + +#if USE_QUADRATIC_EXTRAPOLATION + double variable_delta_sq = variable_delta * variable_delta; + px += mutation_approx_quadratics[itip][variable_index].pos.x() * variable_delta_sq; + py += mutation_approx_quadratics[itip][variable_index].pos.y() * variable_delta_sq; + pz += mutation_approx_quadratics[itip][variable_index].pos.z() * variable_delta_sq; +#endif + + rx += joint_deltas[variable_index].rot.x() * variable_delta; + ry += joint_deltas[variable_index].rot.y() * variable_delta; + rz += joint_deltas[variable_index].rot.z() * variable_delta; + rw += joint_deltas[variable_index].rot.w() * variable_delta; + } + + auto& tip_mutation = tip_frame_mutations[imutation][itip]; + + tip_mutation.pos.setX(px); + tip_mutation.pos.setY(py); + tip_mutation.pos.setZ(pz); + + tip_mutation.rot.setX(rx); + tip_mutation.rot.setY(ry); + tip_mutation.rot.setZ(rz); + tip_mutation.rot.setW(rw); + } + } + } +}; + +#if 0 +class RobotFK_Mutator_2 : public RobotFK_Mutator +{ + struct JointApprox + { + ssize_t mutation_index; + ssize_t variable_index; + Vector3 delta_position; + Vector3 delta_rotation; + size_t link_index; + size_t parent_link_index; + Vector3 link_position; + }; + std::vector joint_approximators; + + struct LinkApprox + { + Vector3 position; + Vector3 rotation; + }; + std::vector link_approximators; + + std::vector link_approx_mask; + + std::vector> variable_to_approximator_index_map; + +public: + RobotFK_Mutator_2(moveit::core::RobotModelConstPtr model) + : RobotFK_Mutator(model) + { + } + + void initializeMutationApproximator(const std::vector& variable_indices) + { + RobotFK_Mutator::initializeMutationApproximator(variable_indices); + + BLOCKPROFILER("initializeMutationApproximator tree"); + + auto tip_count = tip_names.size(); + + link_approximators.resize(robot_model->getLinkModelCount() + 1); + for(auto& l : link_approximators) + { + l.position = Vector3(0, 0, 0); + l.rotation = Vector3(0, 0, 0); + } + + link_approx_mask.clear(); + link_approx_mask.resize(robot_model->getLinkModelCount(), 0); + + joint_approximators.clear(); + + // build joint approximators + for(size_t imut = 0; imut < variable_indices.size(); imut++) + { + size_t ivar = variable_indices[imut]; + auto* joint_model = robot_model->getJointOfVariable(ivar); + { + auto* link_model = joint_model->getChildLinkModel(); + // if(link_approx_mask[link_model->getLinkIndex()]) continue; + joint_approximators.emplace_back(); + auto& joint = joint_approximators.back(); + joint.mutation_index = imut; + joint.variable_index = ivar; + joint.link_index = link_model->getLinkIndex(); + link_approx_mask[link_model->getLinkIndex()] = 1; + } + for(auto* mimic_joint_model : joint_model->getMimicRequests()) + { + // LOG("mimic", mimic_joint_model); + auto* joint_model = mimic_joint_model; + auto* link_model = joint_model->getChildLinkModel(); + // if(link_approx_mask[link_model->getLinkIndex()]) continue; + joint_approximators.emplace_back(); + auto& joint = joint_approximators.back(); + joint.mutation_index = imut; + joint.variable_index = ivar; + joint.link_index = link_model->getLinkIndex(); + link_approx_mask[link_model->getLinkIndex()] = 1; + } + } + + for(auto& joint : joint_approximators) + { + // size_t imut = joint.mutation_index; + // size_t ivar = joint.variable_index; + auto* link_model = robot_model->getLinkModel(joint.link_index); + auto* joint_model = link_model->getParentJointModel(); + joint.delta_position = Vector3(0, 0, 0); + joint.delta_rotation = Vector3(0, 0, 0); + auto& link_frame = global_frames[link_model->getLinkIndex()]; + switch(joint_model->getType()) + { + case moveit::core::JointModel::REVOLUTE: + quat_mul_vec(link_frame.rot, joint_axis_list[joint_model->getJointIndex()], joint.delta_rotation); + break; + case moveit::core::JointModel::PRISMATIC: + quat_mul_vec(link_frame.rot, joint_axis_list[joint_model->getJointIndex()], joint.delta_position); + break; + } + for(auto* j = joint_model; j->getMimic(); j = j->getMimic()) + { + joint.delta_rotation *= j->getMimicFactor(); + joint.delta_position *= j->getMimicFactor(); + break; + } + } + + // continue extrapolation to tip frames, if not already done + for(auto* link_model : tip_links) + { + if(link_approx_mask[link_model->getLinkIndex()]) continue; + joint_approximators.emplace_back(); + auto& joint = joint_approximators.back(); + joint.mutation_index = 0; + joint.variable_index = 0; + joint.link_index = link_model->getLinkIndex(); + joint.delta_position = Vector3(0, 0, 0); + joint.delta_rotation = Vector3(0, 0, 0); + link_approx_mask[link_model->getLinkIndex()] = 1; + } + + std::sort(joint_approximators.begin(), joint_approximators.end(), [](const JointApprox& a, const JointApprox& b) { return a.link_index < b.link_index; }); + + for(auto& joint : joint_approximators) + { + for(auto* link_model = robot_model->getLinkModel(joint.link_index); link_model;) + { + if(link_approx_mask[link_model->getLinkIndex()] >= 2) + { + joint.parent_link_index = link_model->getLinkIndex(); + joint.link_position = global_frames[joint.link_index].pos - global_frames[joint.parent_link_index].pos; + link_approx_mask[joint.link_index] = 2; + break; + } + link_model = link_model->getParentLinkModel(); + if(link_model == 0) + { + joint.parent_link_index = robot_model->getLinkModelCount(); + joint.link_position = global_frames[joint.link_index].pos; + link_approx_mask[joint.link_index] = 2; + break; + } + } + } + + variable_to_approximator_index_map.resize(robot_model->getVariableCount()); + for(auto& l : variable_to_approximator_index_map) + l.clear(); + for(size_t approximator_index = 0; approximator_index < joint_approximators.size(); approximator_index++) + { + if(joint_approximators[approximator_index].variable_index < 0) continue; + variable_to_approximator_index_map[joint_approximators[approximator_index].variable_index].push_back(approximator_index); + } + } + + void computeApproximateMutations(size_t mutation_count, const double* const* mutation_values, std::vector>& tip_frame_mutations) + { + auto tip_count = tip_names.size(); + tip_frame_mutations.resize(mutation_count); + for(auto& m : tip_frame_mutations) + m.resize(tip_count); + for(size_t imutation = 0; imutation < mutation_count; imutation++) + { + for(auto& joint : joint_approximators) + { + double dvar; + /*if(joint.mutation_index < 0) + dvar = 0; + else*/ + dvar = mutation_values[imutation][joint.mutation_index] - variables[joint.variable_index]; + + Vector3 dpos = joint.delta_position * dvar + joint.link_position; + + auto& parent = link_approximators[joint.parent_link_index]; + + // if(dpos.x() || dpos.y() || dpos.z()) + { + Vector3 dpos1 = Vector3(parent.rotation.y() * dpos.z() - parent.rotation.z() * dpos.y(), parent.rotation.z() * dpos.x() - parent.rotation.x() * dpos.z(), parent.rotation.x() * dpos.y() - parent.rotation.y() * dpos.x()); + + /*double xx = 0.5 * parent.rotation.x() * parent.rotation.x(); + double yy = 0.5 * parent.rotation.y() * parent.rotation.y(); + double zz = 0.5 * parent.rotation.z() * parent.rotation.z(); + + Vector3 dpos2 = Vector3( + dpos.x() * (yy + zz), + dpos.y() * (xx + zz), + dpos.z() * (xx + yy) + );*/ + + dpos += dpos1; + } + + // dpos -= dpos2; + + auto& link = link_approximators[joint.link_index]; + link.position = parent.position + dpos; + link.rotation = parent.rotation + joint.delta_rotation * dvar; + } + + for(size_t itip = 0; itip < tip_count; itip++) + { + const Frame& tip_frame = tip_frames[itip]; + auto& tip_mutation = tip_frame_mutations[imutation][itip]; + auto& link = link_approximators[tip_links[itip]->getLinkIndex()]; + tip_mutation.pos = link.position; + + tip_mutation.rot = tf::Quaternion(link.rotation.x() * 0.5, link.rotation.y() * 0.5, link.rotation.z() * 0.5, 1.0) * tip_frame.rot; + + /*tip_mutation.rot = tf::Quaternion( + link.rotation.x() * 0.5, + link.rotation.y() * 0.5, + link.rotation.z() * 0.5, + 1.0 + - link.rotation.length2() * 0.125 + ) * tip_frame.rot;*/ + } + } + } +}; + +typedef RobotFK_Mutator_2 RobotFK; + +#else + +typedef RobotFK_Mutator RobotFK; + +#endif + +#endif + +// for comparison +class RobotFK_MoveIt +{ + moveit::core::RobotModelConstPtr robot_model; + moveit::core::RobotState robot_state; + std::vector tipFrames; + std::vector tipLinks; + std::vector jj; + +public: + RobotFK_MoveIt(moveit::core::RobotModelConstPtr model) + : robot_model(model) + , robot_state(model) + { + } + void initialize(const std::vector& tip_link_indices) + { + tipFrames.resize(tip_link_indices.size()); + tipLinks.resize(tip_link_indices.size()); + for(size_t i = 0; i < tip_link_indices.size(); i++) + tipLinks[i] = robot_model->getLinkModel(tip_link_indices[i]); + } + void applyConfiguration(const std::vector& jj0) + { + BLOCKPROFILER("RobotFK_MoveIt applyConfiguration"); + jj = jj0; + robot_model->interpolate(jj0.data(), jj0.data(), 0.5, jj.data()); // force mimic update + robot_state.setVariablePositions(jj); + robot_state.update(); + for(size_t i = 0; i < tipFrames.size(); i++) + tipFrames[i] = Frame(robot_state.getGlobalLinkTransform(tipLinks[i])); + } + inline void incrementalBegin(const std::vector& jj) {} + inline void incrementalEnd() {} + const Frame& getTipFrame(size_t fi) const { return tipFrames[fi]; } + const std::vector& getTipFrames() const { return tipFrames; } +}; +} diff --git a/src/bio_ik-kinetic-support/src/goal_types.cpp b/src/bio_ik-kinetic-support/src/goal_types.cpp new file mode 100644 index 0000000..a642a58 --- /dev/null +++ b/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 + +#include +#include + +#include + +namespace bio_ik +{ + +void TouchGoal::describe(GoalContext& context) const +{ + LinkGoalBase::describe(context); + auto* robot_model = &context.getRobotModel(); + { + static std::map 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(); + 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(shape)) + { + struct : bodies::ConvexMesh + { + std::vector points; + std::vector polygons; + std::vector plane_normals; + std::vector plane_dis; + void init(const shapes::Shape* shape) + { + type_ = shapes::MESH; + scaled_vertices_ = 0; + { + static std::mutex mutex; + std::lock_guard 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> 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(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(); +} +} diff --git a/src/bio_ik-kinetic-support/src/ik_base.h b/src/bio_ik-kinetic-support/src/ik_base.h new file mode 100644 index 0000000..607bd5a --- /dev/null +++ b/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 + +#include "forward_kinematics.h" +#include "problem.h" +#include "utils.h" +#include + +#include + +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(0, 1)(rng); } + + inline std::size_t random_index(std::size_t s) { return std::uniform_int_distribution(0, s - 1)(rng); } + + std::normal_distribution normal_distribution; + inline double random_gauss() { return normal_distribution(rng); } + + inline double random(double min, double max) { return random() * (max - min) + min; } + + template inline e& random_element(std::vector& l) { return l[random_index(l.size())]; } + + template inline const e& random_element(const std::vector& l) { return l[random_index(l.size())]; } + + XORShift64 _xorshift; + inline size_t fast_random_index(size_t mod) { return _xorshift() % mod; } + template inline const T& fast_random_element(const std::vector& 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 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 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 null_tip_frames; + volatile int canceled; + + virtual void step() = 0; + + virtual const std::vector& 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& variable_positions) { return computeSecondaryFitnessActiveVariables(extractActiveVariables(variable_positions)); } + + double computeFitnessActiveVariables(const std::vector& tip_frames, const double* active_variable_positions) { return problem.computeGoalFitness(problem.goals, tip_frames.data(), active_variable_positions); } + + double computeFitnessActiveVariables(const aligned_vector& tip_frames, const double* active_variable_positions) { return problem.computeGoalFitness(problem.goals, tip_frames.data(), active_variable_positions); } + + double computeCombinedFitnessActiveVariables(const std::vector& 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& 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& tip_frames, const double* active_variable_positions) { return problem.checkSolutionActiveVariables(tip_frames, active_variable_positions); } + + bool checkSolution(const std::vector& variable_positions, const std::vector& tips) { return checkSolutionActiveVariables(tips, extractActiveVariables(variable_positions)); } + + std::vector temp_active_variable_positions; + + double* extractActiveVariables(const std::vector& 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& variable_positions, const std::vector& tip_frames) { return computeFitnessActiveVariables(tip_frames, extractActiveVariables(variable_positions)); } + + double computeFitness(const std::vector& variable_positions) + { + model.applyConfiguration(variable_positions); + return computeFitness(variable_positions, model.getTipFrames()); + } + + virtual size_t concurrency() const { return 1; } +}; + +typedef IKBase IKSolver; + +typedef Factory IKFactory; +} diff --git a/src/bio_ik-kinetic-support/src/ik_cppoptlib.cpp b/src/bio_ik-kinetic-support/src/ik_cppoptlib.cpp new file mode 100644 index 0000000..790483d --- /dev/null +++ b/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 +{ + IKBase* ik; + std::vector 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& state, const TVector& x) + { + // check ik timeout + return ros::WallTime::now().toSec() < ik->problem.timeout; + } +}; +*/ + +// problem description for cppoptlib +struct IKOptLibProblem : cppoptlib::BoundedProblem +{ + IKBase* ik; + std::vector fk_values; + IKOptLibProblem(IKBase* ik) + : cppoptlib::BoundedProblem(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()(x[i]) << i); LOG((void*)h); + return ik->computeFitness(fk_values); + } + bool callback(const cppoptlib::Criteria& state, const TVector& x) + { + // check ik timeout + return ros::WallTime::now().toSec() < ik->problem.timeout; + } +}; + +// ik solver using cppoptlib +template struct IKOptLib : IKBase +{ + // current solution + std::vector solution, best_solution, temp; + + // cppoptlib solver + std::shared_ptr solver; + + // cppoptlib problem description + IKOptLibProblem f; + + // reset flag + bool reset; + + // stop criteria + cppoptlib::Criteria 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::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(); + } + + const std::vector& 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, reset, threads>> ik_optlib_##t##reset##threads(mkoptname(n, reset, threads)); + +//#define IKCPPOPT(n, t) static bio_ik::IKFactory::Class>> 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); diff --git a/src/bio_ik-kinetic-support/src/ik_evolution_1.cpp b/src/bio_ik-kinetic-support/src/ik_evolution_1.cpp new file mode 100644 index 0000000..5435e10 --- /dev/null +++ b/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 genes; + std::vector gradients; + double extinction; + double fitness; + }; + + class HeuristicErrorTree + { + size_t variable_count, tip_count; + std::vector table; + std::vector chain_lengths; + std::vector> chain_lengths_2; + + public: + HeuristicErrorTree() {} + HeuristicErrorTree(moveit::core::RobotModelConstPtr robot_model, const std::vector& 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 solution; + std::vector population; + int populationSize, eliteCount; + std::vector tempPool; + std::vector tempOffspring; + std::vector 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 inline t select(const std::vector& v) + { + // FNPROFILER(); + linear_int_distribution 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& 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 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& 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& 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 cIKEvolution1("bio1"); + +} diff --git a/src/bio_ik-kinetic-support/src/ik_evolution_2.cpp b/src/bio_ik-kinetic-support/src/ik_evolution_2.cpp new file mode 100644 index 0000000..2a4bb8a --- /dev/null +++ b/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 struct IKEvolution2 : IKBase +{ + struct Individual + { + aligned_vector genes; + aligned_vector gradients; + double fitness; + }; + + struct Species + { + std::vector individuals; + double fitness; + bool improved; + }; + + std::vector initial_guess; + std::vector solution; + std::vector temp_joint_variables; + double solution_fitness; + std::vector species; + std::vector children; + std::vector> phenotypes, phenotypes2, phenotypes3; + std::vector child_indices; + std::vector genotypes; + std::vector phenotype; + std::vector quaternion_genes; + aligned_vector genes_min, genes_max, genes_span; + aligned_vector gradient, temp; + + IKEvolution2(const IKParams& p) + : IKBase(p) + { + } + +#ifdef ENABLE_CPP_OPTLIB + struct OptlibProblem : cppoptlib::Problem + { + 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 OptlibSolver; + std::shared_ptr optlib_solver; + std::shared_ptr optlib_problem; + typename OptlibSolver::TVector optlib_vector; +#endif + + void genesToJointVariables(const Individual& individual, std::vector& 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& 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> chain_mutation_masks; + std::vector temp_mutation_chain; + std::vector temp_chain_mutation_var_mask; + */ + + // aligned_vector rmask; + + // create offspring and mutate + __attribute__((hot)) __attribute__((noinline)) + //__attribute__((target_clones("avx2", "avx", "sse2", "default"))) + //__attribute__((target("avx"))) + void + reproduce(const std::vector& 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(); + cppoptlib::Criteria crit; + crit.iterations = 4; + optlib_solver->setStopCriteria(crit); + optlib_problem = std::make_shared(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> bio2("bio2"); +static IKFactory::Class> bio2_memetic("bio2_memetic"); +static IKFactory::Class> bio2_memetic_l("bio2_memetic_l"); + +#ifdef ENABLE_CPP_OPTLIB +static IKFactory::Class> bio2_memetic_0("bio2_memetic_lbfgs"); +#endif +} diff --git a/src/bio_ik-kinetic-support/src/ik_gradient.cpp b/src/bio_ik-kinetic-support/src/ik_gradient.cpp new file mode 100644 index 0000000..cd33c56 --- /dev/null +++ b/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 struct IKJacobianBase : BASE +{ + using BASE::modelInfo; + using BASE::model; + using BASE::params; + using BASE::computeFitness; + using BASE::problem; + + std::vector tipObjectives; + + Eigen::VectorXd tip_diffs; + Eigen::VectorXd joint_diffs; + Eigen::MatrixXd jacobian; + std::vector 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& 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 struct IKGradientDescent : IKBase +{ + std::vector 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& 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> gd("gd"); +static IKFactory::Class> gd_2("gd_2"); +static IKFactory::Class> gd_4("gd_4"); +static IKFactory::Class> gd_8("gd_8"); + +static IKFactory::Class> gd_r("gd_r"); +static IKFactory::Class> gd_2_r("gd_r_2"); +static IKFactory::Class> gd_4_r("gd_r_4"); +static IKFactory::Class> gd_8_r("gd_r_8"); + +static IKFactory::Class> gd_c("gd_c"); +static IKFactory::Class> gd_2_c("gd_c_2"); +static IKFactory::Class> gd_4_c("gd_c_4"); +static IKFactory::Class> gd_8_c("gd_c_8"); + +// pseudoinverse jacobian only +template struct IKJacobian : IKJacobianBase +{ + using IKBase::initialize; + std::vector solution; + IKJacobian(const IKParams& p) + : IKJacobianBase(p) + { + } + void initialize(const Problem& problem) + { + IKJacobianBase::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& getSolution() const { return solution; } + void step() { optimizeJacobian(solution); } + size_t concurrency() const { return threads; } +}; +static IKFactory::Class> jac("jac"); +static IKFactory::Class> jac_2("jac_2"); +static IKFactory::Class> jac_4("jac_4"); +static IKFactory::Class> jac_8("jac_8"); +} diff --git a/src/bio_ik-kinetic-support/src/ik_neural.cpp b/src/bio_ik-kinetic-support/src/ik_neural.cpp new file mode 100644 index 0000000..8c4c2a3 --- /dev/null +++ b/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 +#include + +#include + +#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 solution; + FANN::neural_net net; + + std::vector> input_minmax, output_minmax; + + static void find_minmax(const std::vector& values, std::vector>& 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& values, const std::vector>& 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& values, const std::vector>& 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 levels = {input_count, input_count, input_count, output_count}; + + // std::vector levels = {input_count, input_count + output_count, output_count}; + + // std::vector levels = {input_count, input_count + output_count, input_count + output_count, output_count}; + + // std::vector levels = {input_count, 100, output_count}; + + std::vector levels = {input_count, 50, output_count}; + + net.create_standard_array(levels.size(), levels.data()); + + size_t var_count = params.robot_model->getVariableCount(); + std::vector state1(var_count), state2(var_count); + std::vector frames1, frames2; + + std::vector inputs, outputs; + std::vector 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 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 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& getSolution() const { return solution; } + + std::vector inputs, outputs; + + std::vector 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 neural("neural"); + +struct IKNeural2 : IKBase +{ + std::vector solution; + FANN::neural_net net; + + std::vector> input_minmax, output_minmax; + + /*static void find_minmax(const std::vector& values, std::vector>& 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& values, std::vector>& minmax) + { + std::vector 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 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& values, const std::vector>& 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& values, const std::vector>& 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 levels = {input_count, 100, 100, output_count}; + + // std::vector levels = {input_count, input_count, input_count, output_count}; + + std::vector levels = {input_count, input_count + output_count, output_count}; + + // std::vector levels = {input_count, input_count + output_count, input_count + output_count, output_count}; + + // std::vector levels = {input_count, output_count}; + + net.create_standard_array(levels.size(), levels.data()); + + size_t var_count = params.robot_model->getVariableCount(); + std::vector state = problem.initial_guess; + std::vector frames; + + std::vector inputs, outputs; + std::vector 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 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& getSolution() const { return solution; } + + std::vector inputs, outputs; + + std::vector 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 neural2("neural2"); +} diff --git a/src/bio_ik-kinetic-support/src/ik_parallel.h b/src/bio_ik-kinetic-support/src/ik_parallel.h new file mode 100644 index 0000000..8197ca6 --- /dev/null +++ b/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 + +namespace bio_ik +{ + +// executes a function in parallel on pre-allocated threads +class ParallelExecutor +{ + std::function fun; + std::vector threads; + boost::barrier barrier; + volatile bool exit; + double best_fitness; + +public: + template + 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> solvers; + std::vector> solver_solutions; + std::vector> solver_temps; + std::vector solver_success; + std::vector solver_fitness; + int thread_count; + // std::vector fk; // TODO: remove + double timeout; + bool success; + std::atomic finished; + std::atomic iteration_count; + std::vector result; + std::unique_ptr 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& getSolution() const { return result; } +}; +} diff --git a/src/bio_ik-kinetic-support/src/ik_test.cpp b/src/bio_ik-kinetic-support/src/ik_test.cpp new file mode 100644 index 0000000..f1a07ec --- /dev/null +++ b/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 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& fa, const std::vector& 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> fbm; + + std::vector 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& getSolution() const { return problem.initial_guess; } +}; + +static IKFactory::Class test("test"); +} diff --git a/src/bio_ik-kinetic-support/src/kinematics_plugin.cpp b/src/bio_ik-kinetic-support/src/kinematics_plugin.cpp new file mode 100644 index 0000000..0f5049b --- /dev/null +++ b/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 + +#include "forward_kinematics.h" +#include "ik_base.h" +#include "ik_parallel.h" +#include "problem.h" +#include "utils.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +//#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +using namespace bio_ik; + +// implement BioIKKinematicsQueryOptions + +namespace bio_ik { + +std::mutex bioIKKinematicsQueryOptionsMutex; +std::unordered_set bioIKKinematicsQueryOptionsList; + +BioIKKinematicsQueryOptions::BioIKKinematicsQueryOptions() + : replace(false), solution_fitness(0) { + std::lock_guard lock(bioIKKinematicsQueryOptionsMutex); + bioIKKinematicsQueryOptionsList.insert(this); +} + +BioIKKinematicsQueryOptions::~BioIKKinematicsQueryOptions() { + std::lock_guard lock(bioIKKinematicsQueryOptionsMutex); + bioIKKinematicsQueryOptionsList.erase(this); +} + +bool isBioIKKinematicsQueryOptions(const void *ptr) { + std::lock_guard 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 +static void lookupParam(const std::string ¶m, T &val, + const T &default_val) { + ros::NodeHandle nodeHandle("~"); + val = nodeHandle.param(param, default_val); +} + +struct BioIKKinematicsPlugin : kinematics::KinematicsBase { + std::vector joint_names, link_names; + moveit::core::RobotModelConstPtr robot_model; + const moveit::core::JointModelGroup *joint_model_group; + mutable std::unique_ptr ik; + mutable std::vector state, temp; + mutable std::unique_ptr temp_state; + mutable std::vector tipFrames; + RobotInfo robot_info; + bool enable_profiler; + + BioIKKinematicsPlugin() { enable_profiler = false; } + + virtual const std::vector &getJointNames() const { + LOG_FNC(); + return joint_names; + } + + virtual const std::vector &getLinkNames() const { + LOG_FNC(); + return link_names; + } + + virtual bool getPositionFK(const std::vector &link_names, + const std::vector &joint_angles, + std::vector &poses) const { + LOG_FNC(); + return false; + } + + virtual bool getPositionIK(const geometry_msgs::Pose &ik_pose, + const std::vector &ik_seed_state, + std::vector &solution, + moveit_msgs::MoveItErrorCodes &error_code, + const kinematics::KinematicsQueryOptions &options = + kinematics::KinematicsQueryOptions()) const { + LOG_FNC(); + return false; + } + + std::vector tip_reference_frames; + + mutable std::vector> default_goals; + + mutable std::vector all_goals; + + IKParams ikparams; + + mutable Problem problem; + + static moveit::core::RobotModelConstPtr + loadRobotModel(const std::string &robot_description) { + static std::map + robot_model_cache; + static std::mutex cache_mutex; + std::lock_guard 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 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 &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 &ik_seed_state, double timeout, + std::vector &solution, + moveit_msgs::MoveItErrorCodes &error_code, + const kinematics::KinematicsQueryOptions &options = + kinematics::KinematicsQueryOptions()) const { + LOG_FNC(); + return searchPositionIK(std::vector{ik_pose}, + ik_seed_state, timeout, std::vector(), + solution, IKCallbackFn(), error_code, options); + } + + virtual bool + searchPositionIK(const geometry_msgs::Pose &ik_pose, + const std::vector &ik_seed_state, double timeout, + const std::vector &consistency_limits, + std::vector &solution, + moveit_msgs::MoveItErrorCodes &error_code, + const kinematics::KinematicsQueryOptions &options = + kinematics::KinematicsQueryOptions()) const { + LOG_FNC(); + return searchPositionIK(std::vector{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 &ik_seed_state, double timeout, + std::vector &solution, + const IKCallbackFn &solution_callback, + moveit_msgs::MoveItErrorCodes &error_code, + const kinematics::KinematicsQueryOptions &options = + kinematics::KinematicsQueryOptions()) const { + LOG_FNC(); + return searchPositionIK(std::vector{ik_pose}, + ik_seed_state, timeout, std::vector(), + solution, solution_callback, error_code, options); + } + + virtual bool + searchPositionIK(const geometry_msgs::Pose &ik_pose, + const std::vector &ik_seed_state, double timeout, + const std::vector &consistency_limits, + std::vector &solution, + const IKCallbackFn &solution_callback, + moveit_msgs::MoveItErrorCodes &error_code, + const kinematics::KinematicsQueryOptions &options = + kinematics::KinematicsQueryOptions()) const { + LOG_FNC(); + return searchPositionIK(std::vector{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 &ik_poses, + const std::vector &ik_seed_state, double timeout, + const std::vector &consistency_limits, + std::vector &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> 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); diff --git a/src/bio_ik-kinetic-support/src/problem.cpp b/src/bio_ik-kinetic-support/src/problem.cpp new file mode 100644 index 0000000..ca6759e --- /dev/null +++ b/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 +#include + +#include + +#include + +#include + +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& 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(goal_info.goal)) + { + goal_info.goal_type = GoalType::Position; + goal_info.frame.pos = g->getPosition(); + } + + if(auto* g = dynamic_cast(goal_info.goal)) + { + goal_info.goal_type = GoalType::Orientation; + goal_info.frame.rot = g->getOrientation(); + } + + if(auto* g = dynamic_cast(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& 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& 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; +} +} diff --git a/src/bio_ik-kinetic-support/src/problem.h b/src/bio_ik-kinetic-support/src/problem.h new file mode 100644 index 0000000..da8d260 --- /dev/null +++ b/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 + +#include + +#include + +#include +#include + +#include + +namespace bio_ik +{ + +class Problem +{ +private: + bool ros_params_initrd; + std::vector joint_usage; + std::vector link_tip_indices; + std::vector minimal_displacement_factors; + std::vector 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 vertices; + std::vector points; + std::vector polygons; + std::vector plane_normals; + std::vector plane_dis; + collision_detection::FCLGeometryConstPtr geometry; + Frame frame; + std::vector> edges; + }; + struct CollisionLink + { + bool initialized; + std::vector> shapes; + CollisionLink() + : initialized(false) + { + } + }; + std::vector 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 variable_indices; + mutable size_t last_collision_vertex; + std::vector balance_goal_infos; + };*/ + enum class GoalType; + // std::vector temp_frames; + // std::vector 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 initial_guess; + std::vector active_variables; + std::vector tip_link_indices; + std::vector goals; + std::vector secondary_goals; + Problem(); + void initialize(moveit::core::RobotModelConstPtr robot_model, const moveit::core::JointModelGroup* joint_model_group, const IKParams& params, const std::vector& goals2, const BioIKKinematicsQueryOptions* options); + void initialize2(); + double computeGoalFitness(GoalInfo& goal, const Frame* tip_frames, const double* active_variable_positions); + double computeGoalFitness(std::vector& goals, const Frame* tip_frames, const double* active_variable_positions); + bool checkSolutionActiveVariables(const std::vector& tip_frames, const double* active_variable_positions); +}; +} diff --git a/src/bio_ik-kinetic-support/src/utils.h b/src/bio_ik-kinetic-support/src/utils.h new file mode 100644 index 0000000..a8edc22 --- /dev/null +++ b/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 +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include + +//#include + +//#include +//#include + +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 inline void vprint(std::ostream& s, const T& a) { s << a << std::endl; } +template 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 counter; // only used by CounterScope / COUNTERPROFILER + ProfilerBin() + : name(0) + { + } +}; + +// allocate globally unique profiler buffer via template +template ProfilerBin* getProfilerBuffer() +{ + static std::vector buffer(10000); + return buffer.data(); +} + +// reserve profiler buffer segment for current compilation unit +template 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 ProfilerInfo& getProfilerInfo() +{ + static ProfilerInfo info; + return info; +} +static ProfilerInfo& profiler_info = getProfilerInfo(); + +// profiles a scope or function +template 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 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(rand() % 1000)); + } + { + double thistime = ros::WallTime::now().toSec(); + static double lasttime = 0.0; + if(thistime < lasttime + 1) continue; + lasttime = thistime; + static std::vector> data; + data.clear(); + for(auto& p : samples) + data.push_back(p); + std::sort(data.begin(), data.end(), [](const std::pair& a, const std::pair& 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 linear_int_distribution +{ + std::uniform_int_distribution base; + t n; + +public: + inline linear_int_distribution(t vrange) + : n(vrange) + , base(0, vrange) + { + } + template 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::Class reg("Derived"); +// +// instantiation: +// Base* obj = Factory::create("Derived"); +// +// cloning and object: +// p = Factory::clone(o); +// +template 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 MapType; + static MapType& classes() + { + static MapType ff; + return ff; + } + +public: + template 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 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 struct aligned_allocator : public std::allocator +{ + 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 struct rebind + { + typedef aligned_allocator other; + }; +}; + +// std::vector typedef with proper memory alignment for SIMD operations +template struct aligned_vector : std::vector> +{ +}; + +// 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 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; + } + } +}; +} diff --git a/src/roboglue_ros/CMakeLists.txt b/src/roboglue_ros/CMakeLists.txt new file mode 100644 index 0000000..3080e0e --- /dev/null +++ b/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) diff --git a/src/roboglue_ros/config/data/.data b/src/roboglue_ros/config/data/.data new file mode 100644 index 0000000..38c8b43 --- /dev/null +++ b/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,, + diff --git a/src/roboglue_ros/config/data/@data_template.data b/src/roboglue_ros/config/data/@data_template.data new file mode 100644 index 0000000..ce4e582 --- /dev/null +++ b/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 diff --git a/src/roboglue_ros/config/data/aaaa.data b/src/roboglue_ros/config/data/aaaa.data new file mode 100644 index 0000000..a0c6f87 --- /dev/null +++ b/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 diff --git a/src/roboglue_ros/config/data/abcd.data b/src/roboglue_ros/config/data/abcd.data new file mode 100644 index 0000000..8677949 --- /dev/null +++ b/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 diff --git a/src/roboglue_ros/config/data/test01.data b/src/roboglue_ros/config/data/test01.data new file mode 100644 index 0000000..05a05b8 --- /dev/null +++ b/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 diff --git a/src/roboglue_ros/config/data/test02.data b/src/roboglue_ros/config/data/test02.data new file mode 100644 index 0000000..9d894b5 --- /dev/null +++ b/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 diff --git a/src/roboglue_ros/config/data/test03.data b/src/roboglue_ros/config/data/test03.data new file mode 100644 index 0000000..67caba6 --- /dev/null +++ b/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 diff --git a/src/roboglue_ros/config/data/test04.data b/src/roboglue_ros/config/data/test04.data new file mode 100644 index 0000000..9b263f9 --- /dev/null +++ b/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 diff --git a/src/roboglue_ros/config/data/test05.data b/src/roboglue_ros/config/data/test05.data new file mode 100644 index 0000000..06105a8 --- /dev/null +++ b/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 diff --git a/src/roboglue_ros/config/data/test06.data b/src/roboglue_ros/config/data/test06.data new file mode 100644 index 0000000..c6c1ff4 --- /dev/null +++ b/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 diff --git a/src/roboglue_ros/config/data/test07.data b/src/roboglue_ros/config/data/test07.data new file mode 100644 index 0000000..d69afb3 --- /dev/null +++ b/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 diff --git a/src/roboglue_ros/config/data/test08.data b/src/roboglue_ros/config/data/test08.data new file mode 100644 index 0000000..13a6889 --- /dev/null +++ b/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 diff --git a/src/roboglue_ros/config/data/test09.data b/src/roboglue_ros/config/data/test09.data new file mode 100644 index 0000000..2e98968 --- /dev/null +++ b/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 diff --git a/src/roboglue_ros/config/data/test10.data b/src/roboglue_ros/config/data/test10.data new file mode 100644 index 0000000..712d141 --- /dev/null +++ b/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 diff --git a/src/roboglue_ros/config/data/test11.data b/src/roboglue_ros/config/data/test11.data new file mode 100644 index 0000000..c12f0d7 --- /dev/null +++ b/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 diff --git a/src/roboglue_ros/config/data/test12.data b/src/roboglue_ros/config/data/test12.data new file mode 100644 index 0000000..b1154f2 --- /dev/null +++ b/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 diff --git a/src/roboglue_ros/config/meta/.meta b/src/roboglue_ros/config/meta/.meta new file mode 100644 index 0000000..24ca43e --- /dev/null +++ b/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"} diff --git a/src/roboglue_ros/config/meta/@meta_template.meta b/src/roboglue_ros/config/meta/@meta_template.meta new file mode 100644 index 0000000..61f15ec --- /dev/null +++ b/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 +} diff --git a/src/roboglue_ros/config/meta/aaaa.meta b/src/roboglue_ros/config/meta/aaaa.meta new file mode 100644 index 0000000..38ac24c --- /dev/null +++ b/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"} diff --git a/src/roboglue_ros/config/meta/abcd.meta b/src/roboglue_ros/config/meta/abcd.meta new file mode 100644 index 0000000..ec2177e --- /dev/null +++ b/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"} diff --git a/src/roboglue_ros/config/meta/test01.meta b/src/roboglue_ros/config/meta/test01.meta new file mode 100644 index 0000000..312e22a --- /dev/null +++ b/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"} diff --git a/src/roboglue_ros/config/meta/test02.meta b/src/roboglue_ros/config/meta/test02.meta new file mode 100644 index 0000000..c11b667 --- /dev/null +++ b/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"} diff --git a/src/roboglue_ros/config/meta/test03.meta b/src/roboglue_ros/config/meta/test03.meta new file mode 100644 index 0000000..798acff --- /dev/null +++ b/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"} diff --git a/src/roboglue_ros/config/meta/test04.meta b/src/roboglue_ros/config/meta/test04.meta new file mode 100644 index 0000000..561e82f --- /dev/null +++ b/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"} diff --git a/src/roboglue_ros/config/meta/test05.meta b/src/roboglue_ros/config/meta/test05.meta new file mode 100644 index 0000000..9cca878 --- /dev/null +++ b/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"} diff --git a/src/roboglue_ros/config/meta/test06.meta b/src/roboglue_ros/config/meta/test06.meta new file mode 100644 index 0000000..fab3985 --- /dev/null +++ b/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"} diff --git a/src/roboglue_ros/config/meta/test07.meta b/src/roboglue_ros/config/meta/test07.meta new file mode 100644 index 0000000..b6330af --- /dev/null +++ b/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"} diff --git a/src/roboglue_ros/config/meta/test08.meta b/src/roboglue_ros/config/meta/test08.meta new file mode 100644 index 0000000..09a275a --- /dev/null +++ b/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"} diff --git a/src/roboglue_ros/config/meta/test09.meta b/src/roboglue_ros/config/meta/test09.meta new file mode 100644 index 0000000..b883db0 --- /dev/null +++ b/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"} diff --git a/src/roboglue_ros/config/meta/test10.meta b/src/roboglue_ros/config/meta/test10.meta new file mode 100644 index 0000000..60d1ab1 --- /dev/null +++ b/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"} diff --git a/src/roboglue_ros/config/meta/test11.meta b/src/roboglue_ros/config/meta/test11.meta new file mode 100644 index 0000000..7b4ab9e --- /dev/null +++ b/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"} diff --git a/src/roboglue_ros/config/meta/test12.meta b/src/roboglue_ros/config/meta/test12.meta new file mode 100644 index 0000000..68f57fc --- /dev/null +++ b/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"} diff --git a/src/roboglue_ros/include/roboglue_ros/json.hpp b/src/roboglue_ros/include/roboglue_ros/json.hpp new file mode 100644 index 0000000..c9af0be --- /dev/null +++ b/src/roboglue_ros/include/roboglue_ros/json.hpp @@ -0,0 +1,20406 @@ +/* + __ _____ _____ _____ + __| | __| | | | JSON for Modern C++ +| | |__ | | | | | | version 3.5.0 +|_____|_____|_____|_|___| https://github.com/nlohmann/json + +Licensed under the MIT License . +SPDX-License-Identifier: MIT +Copyright (c) 2013-2018 Niels Lohmann . + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ + +#ifndef NLOHMANN_JSON_HPP +#define NLOHMANN_JSON_HPP + +#define NLOHMANN_JSON_VERSION_MAJOR 3 +#define NLOHMANN_JSON_VERSION_MINOR 5 +#define NLOHMANN_JSON_VERSION_PATCH 0 + +#include // all_of, find, for_each +#include // assert +#include // and, not, or +#include // nullptr_t, ptrdiff_t, size_t +#include // hash, less +#include // initializer_list +#include // istream, ostream +#include // random_access_iterator_tag +#include // accumulate +#include // string, stoi, to_string +#include // declval, forward, move, pair, swap + +// #include +#ifndef NLOHMANN_JSON_FWD_HPP +#define NLOHMANN_JSON_FWD_HPP + +#include // int64_t, uint64_t +#include // map +#include // allocator +#include // string +#include // vector + +/*! +@brief namespace for Niels Lohmann +@see https://github.com/nlohmann +@since version 1.0.0 +*/ +namespace nlohmann +{ +/*! +@brief default JSONSerializer template argument + +This serializer ignores the template arguments and uses ADL +([argument-dependent lookup](https://en.cppreference.com/w/cpp/language/adl)) +for serialization. +*/ +template +struct adl_serializer; + +template class ObjectType = + std::map, + template class ArrayType = std::vector, + class StringType = std::string, class BooleanType = bool, + class NumberIntegerType = std::int64_t, + class NumberUnsignedType = std::uint64_t, + class NumberFloatType = double, + template class AllocatorType = std::allocator, + template class JSONSerializer = + adl_serializer> +class basic_json; + +/*! +@brief JSON Pointer + +A JSON pointer defines a string syntax for identifying a specific value +within a JSON document. It can be used with functions `at` and +`operator[]`. Furthermore, JSON pointers are the base for JSON patches. + +@sa [RFC 6901](https://tools.ietf.org/html/rfc6901) + +@since version 2.0.0 +*/ +template +class json_pointer; + +/*! +@brief default JSON class + +This type is the default specialization of the @ref basic_json class which +uses the standard template types. + +@since version 1.0.0 +*/ +using json = basic_json<>; +} // namespace nlohmann + +#endif + +// #include + + +// This file contains all internal macro definitions +// You MUST include macro_unscope.hpp at the end of json.hpp to undef all of them + +// exclude unsupported compilers +#if !defined(JSON_SKIP_UNSUPPORTED_COMPILER_CHECK) + #if defined(__clang__) + #if (__clang_major__ * 10000 + __clang_minor__ * 100 + __clang_patchlevel__) < 30400 + #error "unsupported Clang version - see https://github.com/nlohmann/json#supported-compilers" + #endif + #elif defined(__GNUC__) && !(defined(__ICC) || defined(__INTEL_COMPILER)) + #if (__GNUC__ * 10000 + __GNUC_MINOR__ * 100 + __GNUC_PATCHLEVEL__) < 40800 + #error "unsupported GCC version - see https://github.com/nlohmann/json#supported-compilers" + #endif + #endif +#endif + +// disable float-equal warnings on GCC/clang +#if defined(__clang__) || defined(__GNUC__) || defined(__GNUG__) + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wfloat-equal" +#endif + +// disable documentation warnings on clang +#if defined(__clang__) + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wdocumentation" +#endif + +// allow for portable deprecation warnings +#if defined(__clang__) || defined(__GNUC__) || defined(__GNUG__) + #define JSON_DEPRECATED __attribute__((deprecated)) +#elif defined(_MSC_VER) + #define JSON_DEPRECATED __declspec(deprecated) +#else + #define JSON_DEPRECATED +#endif + +// allow to disable exceptions +#if (defined(__cpp_exceptions) || defined(__EXCEPTIONS) || defined(_CPPUNWIND)) && !defined(JSON_NOEXCEPTION) + #define JSON_THROW(exception) throw exception + #define JSON_TRY try + #define JSON_CATCH(exception) catch(exception) + #define JSON_INTERNAL_CATCH(exception) catch(exception) +#else + #define JSON_THROW(exception) std::abort() + #define JSON_TRY if(true) + #define JSON_CATCH(exception) if(false) + #define JSON_INTERNAL_CATCH(exception) if(false) +#endif + +// override exception macros +#if defined(JSON_THROW_USER) + #undef JSON_THROW + #define JSON_THROW JSON_THROW_USER +#endif +#if defined(JSON_TRY_USER) + #undef JSON_TRY + #define JSON_TRY JSON_TRY_USER +#endif +#if defined(JSON_CATCH_USER) + #undef JSON_CATCH + #define JSON_CATCH JSON_CATCH_USER + #undef JSON_INTERNAL_CATCH + #define JSON_INTERNAL_CATCH JSON_CATCH_USER +#endif +#if defined(JSON_INTERNAL_CATCH_USER) + #undef JSON_INTERNAL_CATCH + #define JSON_INTERNAL_CATCH JSON_INTERNAL_CATCH_USER +#endif + +// manual branch prediction +#if defined(__clang__) || defined(__GNUC__) || defined(__GNUG__) + #define JSON_LIKELY(x) __builtin_expect(!!(x), 1) + #define JSON_UNLIKELY(x) __builtin_expect(!!(x), 0) +#else + #define JSON_LIKELY(x) x + #define JSON_UNLIKELY(x) x +#endif + +// C++ language standard detection +#if (defined(__cplusplus) && __cplusplus >= 201703L) || (defined(_HAS_CXX17) && _HAS_CXX17 == 1) // fix for issue #464 + #define JSON_HAS_CPP_17 + #define JSON_HAS_CPP_14 +#elif (defined(__cplusplus) && __cplusplus >= 201402L) || (defined(_HAS_CXX14) && _HAS_CXX14 == 1) + #define JSON_HAS_CPP_14 +#endif + +/*! +@brief macro to briefly define a mapping between an enum and JSON +@def NLOHMANN_JSON_SERIALIZE_ENUM +@since version 3.4.0 +*/ +#define NLOHMANN_JSON_SERIALIZE_ENUM(ENUM_TYPE, ...) \ + template \ + inline void to_json(BasicJsonType& j, const ENUM_TYPE& e) \ + { \ + static_assert(std::is_enum::value, #ENUM_TYPE " must be an enum!"); \ + static const std::pair m[] = __VA_ARGS__; \ + auto it = std::find_if(std::begin(m), std::end(m), \ + [e](const std::pair& ej_pair) -> bool \ + { \ + return ej_pair.first == e; \ + }); \ + j = ((it != std::end(m)) ? it : std::begin(m))->second; \ + } \ + template \ + inline void from_json(const BasicJsonType& j, ENUM_TYPE& e) \ + { \ + static_assert(std::is_enum::value, #ENUM_TYPE " must be an enum!"); \ + static const std::pair m[] = __VA_ARGS__; \ + auto it = std::find_if(std::begin(m), std::end(m), \ + [j](const std::pair& ej_pair) -> bool \ + { \ + return ej_pair.second == j; \ + }); \ + e = ((it != std::end(m)) ? it : std::begin(m))->first; \ + } + +// Ugly macros to avoid uglier copy-paste when specializing basic_json. They +// may be removed in the future once the class is split. + +#define NLOHMANN_BASIC_JSON_TPL_DECLARATION \ + template class ObjectType, \ + template class ArrayType, \ + class StringType, class BooleanType, class NumberIntegerType, \ + class NumberUnsignedType, class NumberFloatType, \ + template class AllocatorType, \ + template class JSONSerializer> + +#define NLOHMANN_BASIC_JSON_TPL \ + basic_json + +// #include + + +#include // not +#include // size_t +#include // conditional, enable_if, false_type, integral_constant, is_constructible, is_integral, is_same, remove_cv, remove_reference, true_type + +namespace nlohmann +{ +namespace detail +{ +// alias templates to reduce boilerplate +template +using enable_if_t = typename std::enable_if::type; + +template +using uncvref_t = typename std::remove_cv::type>::type; + +// implementation of C++14 index_sequence and affiliates +// source: https://stackoverflow.com/a/32223343 +template +struct index_sequence +{ + using type = index_sequence; + using value_type = std::size_t; + static constexpr std::size_t size() noexcept + { + return sizeof...(Ints); + } +}; + +template +struct merge_and_renumber; + +template +struct merge_and_renumber, index_sequence> + : index_sequence < I1..., (sizeof...(I1) + I2)... > {}; + +template +struct make_index_sequence + : merge_and_renumber < typename make_index_sequence < N / 2 >::type, + typename make_index_sequence < N - N / 2 >::type > {}; + +template<> struct make_index_sequence<0> : index_sequence<> {}; +template<> struct make_index_sequence<1> : index_sequence<0> {}; + +template +using index_sequence_for = make_index_sequence; + +// dispatch utility (taken from ranges-v3) +template struct priority_tag : priority_tag < N - 1 > {}; +template<> struct priority_tag<0> {}; + +// taken from ranges-v3 +template +struct static_const +{ + static constexpr T value{}; +}; + +template +constexpr T static_const::value; +} // namespace detail +} // namespace nlohmann + +// #include + + +#include // not +#include // numeric_limits +#include // false_type, is_constructible, is_integral, is_same, true_type +#include // declval + +// #include + +// #include + + +#include // random_access_iterator_tag + +// #include + + +namespace nlohmann +{ +namespace detail +{ +template struct make_void +{ + using type = void; +}; +template using void_t = typename make_void::type; +} // namespace detail +} // namespace nlohmann + +// #include + + +namespace nlohmann +{ +namespace detail +{ +template +struct iterator_types {}; + +template +struct iterator_types < + It, + void_t> +{ + using difference_type = typename It::difference_type; + using value_type = typename It::value_type; + using pointer = typename It::pointer; + using reference = typename It::reference; + using iterator_category = typename It::iterator_category; +}; + +// This is required as some compilers implement std::iterator_traits in a way that +// doesn't work with SFINAE. See https://github.com/nlohmann/json/issues/1341. +template +struct iterator_traits +{ +}; + +template +struct iterator_traits < T, enable_if_t < !std::is_pointer::value >> + : iterator_types +{ +}; + +template +struct iterator_traits::value>> +{ + using iterator_category = std::random_access_iterator_tag; + using value_type = T; + using difference_type = ptrdiff_t; + using pointer = T*; + using reference = T&; +}; +} +} + +// #include + +// #include + + +#include + +// #include + + +// http://en.cppreference.com/w/cpp/experimental/is_detected +namespace nlohmann +{ +namespace detail +{ +struct nonesuch +{ + nonesuch() = delete; + ~nonesuch() = delete; + nonesuch(nonesuch const&) = delete; + void operator=(nonesuch const&) = delete; +}; + +template class Op, + class... Args> +struct detector +{ + using value_t = std::false_type; + using type = Default; +}; + +template class Op, class... Args> +struct detector>, Op, Args...> +{ + using value_t = std::true_type; + using type = Op; +}; + +template