diff --git a/.travis.yml b/.travis.yml
index 61f470de..c2d20273 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -16,12 +16,13 @@ install:
- sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu xenial main" > /etc/apt/sources.list.d/ros-latest.list'
- wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
- sudo apt-get update -qq
- - sudo apt-get install -qq -y python-rosdep python-catkin-tools
+ - sudo apt-get install -qq -y python-rosdep python-catkin-tools python3-vcstool
- sudo rosdep init
- rosdep update
# Use rosdep to install all dependencies (including ROS itself)
- git clone https://github.com/RoboticaUtnFrba/i2c_imu.git $TRAVIS_BUILD_DIR/i2c_imu
- git clone https://github.com/RoboticaUtnFrba/libcreate.git $TRAVIS_BUILD_DIR/libcreate
+ - git clone https://github.com/RoboticaUtnFrba/mbf_tools.git $TRAVIS_BUILD_DIR/mbf_tools
- rosdep install --from-paths ./ -i -y --rosdistro $CI_ROS_DISTRO
# Install and compile RTIMULib2
- git clone https://github.com/RoboticaUtnFrba/RTIMULib2.git $TRAVIS_BUILD_DIR/RTIMULib2
diff --git a/docker/create_kinetic_nvidia/Dockerfile b/docker/create_kinetic_nvidia/Dockerfile
index e0b19be8..a8fa6c59 100644
--- a/docker/create_kinetic_nvidia/Dockerfile
+++ b/docker/create_kinetic_nvidia/Dockerfile
@@ -15,7 +15,23 @@ ENV NVIDIA_VISIBLE_DEVICES=all NVIDIA_DRIVER_CAPABILITIES=all
USER root
RUN apt-get update
+# Added dependencies
RUN apt-get install -y \
- ros-$ROS1_DISTRO-image-proc
+ libopencv-dev \
+ ros-$ROS1_DISTRO-dwa-local-planner \
+ ros-$ROS1_DISTRO-eband-local-planner \
+ ros-$ROS1_DISTRO-global-planner \
+ ros-$ROS1_DISTRO-image-proc \
+ ros-$ROS1_DISTRO-joy \
+ ros-$ROS1_DISTRO-joy-teleop \
+ ros-$ROS1_DISTRO-libdlib \
+ ros-$ROS1_DISTRO-map-server \
+ ros-$ROS1_DISTRO-mbf-costmap-core \
+ ros-$ROS1_DISTRO-mbf-msgs \
+ ros-$ROS1_DISTRO-move-base-flex \
+ ros-$ROS1_DISTRO-opengm \
+ ros-$ROS1_DISTRO-sbpl-lattice-planner \
+ ros-$ROS1_DISTRO-slam-gmapping \
+ ros-$ROS1_DISTRO-smach-viewer
USER create
diff --git a/docker/create_melodic_nvidia/Dockerfile b/docker/create_melodic_nvidia/Dockerfile
index 220fd1bf..bc8aa132 100644
--- a/docker/create_melodic_nvidia/Dockerfile
+++ b/docker/create_melodic_nvidia/Dockerfile
@@ -15,7 +15,23 @@ ENV NVIDIA_VISIBLE_DEVICES=all NVIDIA_DRIVER_CAPABILITIES=all
USER root
RUN apt-get update
+# Added dependencies
RUN apt-get install -y \
- ros-$ROS1_DISTRO-image-proc
+ libopencv-dev \
+ ros-$ROS1_DISTRO-dwa-local-planner \
+ # ros-$ROS1_DISTRO-eband-local-planner \
+ ros-$ROS1_DISTRO-global-planner \
+ ros-$ROS1_DISTRO-image-proc \
+ ros-$ROS1_DISTRO-joy \
+ ros-$ROS1_DISTRO-joy-teleop \
+ # ros-$ROS1_DISTRO-libdlib \
+ ros-$ROS1_DISTRO-map-server \
+ ros-$ROS1_DISTRO-mbf-costmap-core \
+ ros-$ROS1_DISTRO-mbf-msgs \
+ ros-$ROS1_DISTRO-move-base-flex \
+ # ros-$ROS1_DISTRO-opengm \
+ ros-$ROS1_DISTRO-sbpl-lattice-planner \
+ # ros-$ROS1_DISTRO-slam-gmapping \
+ ros-$ROS1_DISTRO-smach-viewer
USER create
diff --git a/navigation/ca_move_base/CMakeLists.txt b/navigation/ca_move_base/CMakeLists.txt
index 1264dc23..3d1af44c 100644
--- a/navigation/ca_move_base/CMakeLists.txt
+++ b/navigation/ca_move_base/CMakeLists.txt
@@ -16,9 +16,6 @@ find_package(catkin REQUIRED COMPONENTS
tf
)
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
###################################
## catkin specific configuration ##
###################################
@@ -28,12 +25,7 @@ find_package(catkin REQUIRED COMPONENTS
## 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 ca_move_base
-# CATKIN_DEPENDS actionlib move_base move_base_msgs roscpp tf
-# DEPENDS system_lib
-)
+catkin_package()
###########
## Build ##
@@ -46,80 +38,12 @@ include_directories(
${catkin_INCLUDE_DIRS}
)
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-# src/${PROJECT_NAME}/ca_move_base.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(send_goal src/send_robot_goal.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(send_goal
${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_script
-# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables and/or libraries for installation
-# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
-# 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_ca_move_base.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/navigation/ca_move_base/config/controllers.yaml b/navigation/ca_move_base/config/controllers.yaml
new file mode 100644
index 00000000..62e692a8
--- /dev/null
+++ b/navigation/ca_move_base/config/controllers.yaml
@@ -0,0 +1,101 @@
+# controllers:
+# - name: 'eband'
+# type: 'eband_local_planner/EBandPlannerROS'
+# - name: 'dwa'
+# type: 'dwa_local_planner/DWAPlannerROS'
+# eband:
+# Ctrl_Rate: 20.0
+# bubble_velocity_multiplier: 2.0
+# costmap_weight: 10.0
+# differential_drive: true
+# disallow_hysteresis: false
+# eband_equilibrium_approx_max_recursion_depth: 3
+# eband_equilibrium_relative_overshoot: 0.75
+# eband_external_force_gain: 3.0
+# eband_internal_force_gain: 2.0
+# eband_min_relative_overlap: 0.7
+# eband_significant_force_lower_bound: 0.15
+# eband_tiny_bubble_distance: 0.01
+# eband_tiny_bubble_expansion: 0.01
+# in_place_trans_vel: 0.0
+# k_damp: 3.5
+# k_prop: 4.0
+# marker_lifetime: 0.5
+# max_acceleration: 0.2
+# max_rotational_acceleration: 0.3
+# max_translational_acceleration: 0.4
+# # Lower values for testing with the actual robot.
+# max_vel_lin: 1.1 #0.5 #1.45
+# max_vel_th: 1.2 #0.5 #2.0
+# min_in_place_vel_th: 0.05
+# min_vel_lin: 0.0
+# min_vel_th: 0.0
+# num_iterations_eband_optimization: 4
+# rot_stopped_vel: 0.01
+# rotation_correction_threshold: 0.5
+# rotation_threshold_multiplier: 1.0
+# trans_stopped_vel: 0.01
+# virtual_mass: 100.0
+# xy_goal_tolerance: 0.2
+# yaw_goal_tolerance: 0.2
+
+# dwa:
+# max_vel_x: 2.0
+# min_vel_x: 0.0
+
+# max_vel_y: 0.0
+# min_vel_y: 0.0
+
+# max_trans_vel: 2.12
+# min_trans_vel: 0.1 # this is the min trans velocity when there is negligible rotational velocity
+# trans_stopped_vel: 0.1
+
+# # Warning!
+# # do not set min_trans_vel to 0.0 otherwise dwa will always think translational velocities
+# # are non-negligible and small in place rotational velocities will be created.
+
+# max_rot_vel: 2.0 # choose slightly less than the base's capability
+# min_rot_vel: 0.4 # this is the min angular velocity when there is negligible translational velocity
+# rot_stopped_vel: 0.1
+
+# acc_lim_x: 0.7 # maximum is theoretically 2.0, but we
+# acc_lim_theta: 10.0
+# acc_lim_y: 0.1 # diff drive robot
+
+# # Goal Tolerance Parameters
+# yaw_goal_tolerance: 0.4 # 0.05
+# xy_goal_tolerance: 0.2 # 0.10
+# # latch_xy_goal_tolerance: false
+
+# # Forward Simulation Parameters
+# sim_time: 2.0 # 1.7
+# sim_granularity: 0.025
+# angular_sim_granularity: 0.025
+# vx_samples: 20 # 3
+# vy_samples: 1 # diff drive robot, there is only one sample
+# vtheta_samples: 20 # 20
+
+# # Trajectory Scoring Parameters
+# path_distance_bias: 0.1 # 32.0 - weighting for how much it should stick to the global path plan -- if this value is too high, won't avoid dynamic obstacles
+# goal_distance_bias: 40.0 # 24.0 - wighting for how much it should attempt to reach its goal
+# occdist_scale: 0.2 # 0.01 - weighting for how much the controller should avoid obstacles
+# forward_point_distance: 0.0 # 0.325 - how far along to place an additional scoring point
+# stop_time_buffer: 0.3 # 0.2 - amount of time a robot must stop in before colliding for a valid traj.
+# scaling_speed: 1.0 # 0.25 - absolute velocity at which to start scaling the robot's footprint
+# max_scaling_factor: 0.2 # 0.2 - how much to scale the robot's footprint when at speed.
+
+# # Oscillation Prevention Parameters
+# oscillation_reset_dist: 0.1 # 0.05 - how far to travel before resetting oscillation flags
+
+# holonomic_robot: false
+
+# # Debugging
+# publish_traj_pc: true
+# publish_cost_grid_pc: true
+# global_frame_id: map
+
+controllers:
+ - name: 'controller'
+ type: 'base_local_planner/TrajectoryPlannerROS'
+controller:
+ holonomic_robot: false
diff --git a/navigation/ca_move_base/config/planners.yaml b/navigation/ca_move_base/config/planners.yaml
new file mode 100644
index 00000000..cd3d3529
--- /dev/null
+++ b/navigation/ca_move_base/config/planners.yaml
@@ -0,0 +1,38 @@
+# planners:
+# - name: 'GlobalPlanner'
+# type: 'global_planner/GlobalPlanner'
+# - name: 'SBPLLatticePlanner'
+# type: 'SBPLLatticePlanner'
+# planner_patience: 10.0
+# GlobalPlanner:
+# allow_unknown: false
+# default_tolerance: 0.0
+# visualize_potential: true
+# use_dijkstra: true
+# use_quadratic: false
+# use_grid_path: false
+# old_navfn_behaviour: false
+# lethal_cost: 253
+# neutral_cost: 66
+# cost_factor: 0.55
+# SBPLLatticePlanner:
+# planner_type: 'ARAPlanner'
+# allocated_time: 20.0 # default: 10.0
+# initial_epsilon: 3.0
+# environment_type: 'XYThetaLattice' # The only environment supported
+# forward_search: true # default is false
+# nominalvel_mpersecs: 0.7
+# timetoturn45degsinplace_secs: 0.6
+# lethal_obstacle: 250
+
+planners:
+ - name: 'planner'
+ type: 'global_planner/GlobalPlanner'
+planner:
+ allow_unknown: false
+ default_tolerance: 0.0
+ visualize_potential: false
+ use_dijkstra: true
+ use_quadratic: false
+ use_grid_path: false
+ old_navfn_behaviour: false
diff --git a/navigation/ca_move_base/config/recovery_behaviors.yaml b/navigation/ca_move_base/config/recovery_behaviors.yaml
new file mode 100644
index 00000000..1d7126ce
--- /dev/null
+++ b/navigation/ca_move_base/config/recovery_behaviors.yaml
@@ -0,0 +1,12 @@
+recovery_behaviors:
+ - name: 'rotate_recovery'
+ type: 'rotate_recovery/RotateRecovery'
+ - name: 'clear_costmap'
+ type: 'clear_costmap_recovery/ClearCostmapRecovery'
+ - name: 'moveback_recovery'
+ type: 'moveback_recovery/MoveBackRecovery'
+
+moveback_recovery:
+ linear_vel_back: -0.1 # default -0.3
+ step_back_length: 0.3 # default 1.0
+ step_back_timeout: 5.0 # default 15.0
diff --git a/navigation/ca_move_base/launch/move_base_flex.launch b/navigation/ca_move_base/launch/move_base_flex.launch
new file mode 100644
index 00000000..d85bf839
--- /dev/null
+++ b/navigation/ca_move_base/launch/move_base_flex.launch
@@ -0,0 +1,15 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/navigation/ca_move_base/launch/navigate.launch b/navigation/ca_move_base/launch/navigate.launch
index f5f0c68e..86da2356 100644
--- a/navigation/ca_move_base/launch/navigate.launch
+++ b/navigation/ca_move_base/launch/navigate.launch
@@ -9,7 +9,10 @@
-
+
+
+
+
diff --git a/navigation/ca_move_base/mprim/ceres_5cm.mprim b/navigation/ca_move_base/mprim/ceres_5cm.mprim
new file mode 100644
index 00000000..16071bcd
--- /dev/null
+++ b/navigation/ca_move_base/mprim/ceres_5cm.mprim
@@ -0,0 +1,2403 @@
+resolution_m: 0.050000
+numberofangles: 16
+totalnumberofprimitives: 160
+primID: 0
+startangle_c: 0
+endpose_c: 1 0 0
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 0.0000
+0.0056 0.0000 0.0000
+0.0111 0.0000 0.0000
+0.0167 0.0000 0.0000
+0.0222 0.0000 0.0000
+0.0278 0.0000 0.0000
+0.0333 0.0000 0.0000
+0.0389 0.0000 0.0000
+0.0444 0.0000 0.0000
+0.0500 0.0000 0.0000
+primID: 1
+startangle_c: 0
+endpose_c: 8 0 0
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 0.0000
+0.0444 0.0000 0.0000
+0.0889 0.0000 0.0000
+0.1333 0.0000 0.0000
+0.1778 0.0000 0.0000
+0.2222 0.0000 0.0000
+0.2667 0.0000 0.0000
+0.3111 0.0000 0.0000
+0.3556 0.0000 0.0000
+0.4000 0.0000 0.0000
+primID: 2
+startangle_c: 0
+endpose_c: -1 0 0
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 0.0000
+-0.0056 0.0000 0.0000
+-0.0111 0.0000 0.0000
+-0.0167 0.0000 0.0000
+-0.0222 0.0000 0.0000
+-0.0278 0.0000 0.0000
+-0.0333 0.0000 0.0000
+-0.0389 0.0000 0.0000
+-0.0444 0.0000 0.0000
+-0.0500 0.0000 0.0000
+primID: 3
+startangle_c: 0
+endpose_c: 8 1 1
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 0.0000
+0.0452 -0.0000 0.0000
+0.0904 -0.0000 0.0000
+0.1355 -0.0000 0.0000
+0.1807 0.0008 0.0488
+0.2257 0.0045 0.1176
+0.2703 0.0114 0.1864
+0.3144 0.0213 0.2551
+0.3577 0.0342 0.3239
+0.4000 0.0500 0.3927
+primID: 4
+startangle_c: 0
+endpose_c: 8 -1 -1
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 0.0000
+0.0452 0.0000 0.0000
+0.0904 0.0000 0.0000
+0.1355 0.0000 0.0000
+0.1807 -0.0008 -0.0488
+0.2257 -0.0045 -0.1176
+0.2703 -0.0114 -0.1864
+0.3144 -0.0213 -0.2551
+0.3577 -0.0342 -0.3239
+0.4000 -0.0500 -0.3927
+primID: 5
+startangle_c: 0
+endpose_c: 0 0 1
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 0.0000
+0.0000 0.0000 0.0436
+0.0000 0.0000 0.0873
+0.0000 0.0000 0.1309
+0.0000 0.0000 0.1745
+0.0000 0.0000 0.2182
+0.0000 0.0000 0.2618
+0.0000 0.0000 0.3054
+0.0000 0.0000 0.3491
+0.0000 0.0000 0.3927
+primID: 6
+startangle_c: 0
+endpose_c: 0 0 -1
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 0.0000
+0.0000 0.0000 -0.0436
+0.0000 0.0000 -0.0873
+0.0000 0.0000 -0.1309
+0.0000 0.0000 -0.1745
+0.0000 0.0000 -0.2182
+0.0000 0.0000 -0.2618
+0.0000 0.0000 -0.3054
+0.0000 0.0000 -0.3491
+0.0000 0.0000 -0.3927
+primID: 7
+startangle_c: 0
+endpose_c: 0 0 0
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 0.0000
+0.0000 0.0000 0.0000
+0.0000 0.0000 0.0000
+0.0000 0.0000 0.0000
+0.0000 0.0000 0.0000
+0.0000 0.0000 0.0000
+0.0000 0.0000 0.0000
+0.0000 0.0000 0.0000
+0.0000 0.0000 0.0000
+0.0000 0.0000 0.0000
+primID: 8
+startangle_c: 0
+endpose_c: 0 0 0
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 0.0000
+0.0000 0.0000 0.0000
+0.0000 0.0000 0.0000
+0.0000 0.0000 0.0000
+0.0000 0.0000 0.0000
+0.0000 0.0000 0.0000
+0.0000 0.0000 0.0000
+0.0000 0.0000 0.0000
+0.0000 0.0000 0.0000
+0.0000 0.0000 0.0000
+primID: 9
+startangle_c: 0
+endpose_c: 0 0 0
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 0.0000
+0.0000 0.0000 0.0000
+0.0000 0.0000 0.0000
+0.0000 0.0000 0.0000
+0.0000 0.0000 0.0000
+0.0000 0.0000 0.0000
+0.0000 0.0000 0.0000
+0.0000 0.0000 0.0000
+0.0000 0.0000 0.0000
+0.0000 0.0000 0.0000
+primID: 0
+startangle_c: 1
+endpose_c: 2 1 1
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 0.3927
+0.0111 0.0056 0.3927
+0.0222 0.0111 0.3927
+0.0333 0.0167 0.3927
+0.0444 0.0222 0.3927
+0.0556 0.0278 0.3927
+0.0667 0.0333 0.3927
+0.0778 0.0389 0.3927
+0.0889 0.0444 0.3927
+0.1000 0.0500 0.3927
+primID: 1
+startangle_c: 1
+endpose_c: 6 3 1
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 0.3927
+0.0333 0.0167 0.3927
+0.0667 0.0333 0.3927
+0.1000 0.0500 0.3927
+0.1333 0.0667 0.3927
+0.1667 0.0833 0.3927
+0.2000 0.1000 0.3927
+0.2333 0.1167 0.3927
+0.2667 0.1333 0.3927
+0.3000 0.1500 0.3927
+primID: 2
+startangle_c: 1
+endpose_c: -2 -1 1
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 0.3927
+-0.0111 -0.0056 0.3927
+-0.0222 -0.0111 0.3927
+-0.0333 -0.0167 0.3927
+-0.0444 -0.0222 0.3927
+-0.0556 -0.0278 0.3927
+-0.0667 -0.0333 0.3927
+-0.0778 -0.0389 0.3927
+-0.0889 -0.0444 0.3927
+-0.1000 -0.0500 0.3927
+primID: 3
+startangle_c: 1
+endpose_c: 5 4 2
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 0.3927
+0.0298 0.0184 0.4230
+0.0592 0.0379 0.4533
+0.0881 0.0583 0.4836
+0.1165 0.0796 0.5139
+0.1444 0.1019 0.5442
+0.1717 0.1251 0.5745
+0.1984 0.1492 0.6048
+0.2245 0.1742 0.6351
+0.2500 0.2000 0.6654
+primID: 4
+startangle_c: 1
+endpose_c: 7 2 0
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 0.3927
+0.0377 0.0156 0.3927
+0.0754 0.0312 0.3927
+0.1131 0.0468 0.3927
+0.1508 0.0623 0.3736
+0.1893 0.0758 0.2989
+0.2287 0.0863 0.2242
+0.2687 0.0939 0.1494
+0.3092 0.0985 0.0747
+0.3500 0.1000 -0.0000
+primID: 5
+startangle_c: 1
+endpose_c: 0 0 2
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 0.3927
+0.0000 0.0000 0.4363
+0.0000 0.0000 0.4800
+0.0000 0.0000 0.5236
+0.0000 0.0000 0.5672
+0.0000 0.0000 0.6109
+0.0000 0.0000 0.6545
+0.0000 0.0000 0.6981
+0.0000 0.0000 0.7418
+0.0000 0.0000 0.7854
+primID: 6
+startangle_c: 1
+endpose_c: 0 0 0
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 0.3927
+0.0000 0.0000 0.3491
+0.0000 0.0000 0.3054
+0.0000 0.0000 0.2618
+0.0000 0.0000 0.2182
+0.0000 0.0000 0.1745
+0.0000 0.0000 0.1309
+0.0000 0.0000 0.0873
+0.0000 0.0000 0.0436
+0.0000 0.0000 0.0000
+primID: 7
+startangle_c: 1
+endpose_c: 0 0 1
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 0.3927
+0.0000 0.0000 0.3927
+0.0000 0.0000 0.3927
+0.0000 0.0000 0.3927
+0.0000 0.0000 0.3927
+0.0000 0.0000 0.3927
+0.0000 0.0000 0.3927
+0.0000 0.0000 0.3927
+0.0000 0.0000 0.3927
+0.0000 0.0000 0.3927
+primID: 8
+startangle_c: 1
+endpose_c: 0 0 1
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 0.3927
+0.0000 0.0000 0.3927
+0.0000 0.0000 0.3927
+0.0000 0.0000 0.3927
+0.0000 0.0000 0.3927
+0.0000 0.0000 0.3927
+0.0000 0.0000 0.3927
+0.0000 0.0000 0.3927
+0.0000 0.0000 0.3927
+0.0000 0.0000 0.3927
+primID: 9
+startangle_c: 1
+endpose_c: 0 0 1
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 0.3927
+0.0000 0.0000 0.3927
+0.0000 0.0000 0.3927
+0.0000 0.0000 0.3927
+0.0000 0.0000 0.3927
+0.0000 0.0000 0.3927
+0.0000 0.0000 0.3927
+0.0000 0.0000 0.3927
+0.0000 0.0000 0.3927
+0.0000 0.0000 0.3927
+primID: 0
+startangle_c: 2
+endpose_c: 1 1 2
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 0.7854
+0.0056 0.0056 0.7854
+0.0111 0.0111 0.7854
+0.0167 0.0167 0.7854
+0.0222 0.0222 0.7854
+0.0278 0.0278 0.7854
+0.0333 0.0333 0.7854
+0.0389 0.0389 0.7854
+0.0444 0.0444 0.7854
+0.0500 0.0500 0.7854
+primID: 1
+startangle_c: 2
+endpose_c: 6 6 2
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 0.7854
+0.0333 0.0333 0.7854
+0.0667 0.0667 0.7854
+0.1000 0.1000 0.7854
+0.1333 0.1333 0.7854
+0.1667 0.1667 0.7854
+0.2000 0.2000 0.7854
+0.2333 0.2333 0.7854
+0.2667 0.2667 0.7854
+0.3000 0.3000 0.7854
+primID: 2
+startangle_c: 2
+endpose_c: -1 -1 2
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 0.7854
+-0.0056 -0.0056 0.7854
+-0.0111 -0.0111 0.7854
+-0.0167 -0.0167 0.7854
+-0.0222 -0.0222 0.7854
+-0.0278 -0.0278 0.7854
+-0.0333 -0.0333 0.7854
+-0.0389 -0.0389 0.7854
+-0.0444 -0.0444 0.7854
+-0.0500 -0.0500 0.7854
+primID: 3
+startangle_c: 2
+endpose_c: 5 7 3
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 0.7854
+0.0341 0.0341 0.7854
+0.0678 0.0684 0.8151
+0.1000 0.1043 0.8669
+0.1302 0.1418 0.9188
+0.1584 0.1809 0.9707
+0.1846 0.2213 1.0225
+0.2086 0.2631 1.0744
+0.2304 0.3060 1.1262
+0.2500 0.3500 1.1781
+primID: 4
+startangle_c: 2
+endpose_c: 7 5 1
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 0.7854
+0.0341 0.0341 0.7854
+0.0684 0.0678 0.7557
+0.1043 0.1000 0.7039
+0.1418 0.1302 0.6520
+0.1809 0.1584 0.6001
+0.2213 0.1846 0.5483
+0.2631 0.2086 0.4964
+0.3060 0.2304 0.4446
+0.3500 0.2500 0.3927
+primID: 5
+startangle_c: 2
+endpose_c: 0 0 3
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 0.7854
+0.0000 0.0000 0.8290
+0.0000 0.0000 0.8727
+0.0000 0.0000 0.9163
+0.0000 0.0000 0.9599
+0.0000 0.0000 1.0036
+0.0000 0.0000 1.0472
+0.0000 0.0000 1.0908
+0.0000 0.0000 1.1345
+0.0000 0.0000 1.1781
+primID: 6
+startangle_c: 2
+endpose_c: 0 0 1
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 0.7854
+0.0000 0.0000 0.7418
+0.0000 0.0000 0.6981
+0.0000 0.0000 0.6545
+0.0000 0.0000 0.6109
+0.0000 0.0000 0.5672
+0.0000 0.0000 0.5236
+0.0000 0.0000 0.4800
+0.0000 0.0000 0.4363
+0.0000 0.0000 0.3927
+primID: 7
+startangle_c: 2
+endpose_c: 0 0 2
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 0.7854
+0.0000 0.0000 0.7854
+0.0000 0.0000 0.7854
+0.0000 0.0000 0.7854
+0.0000 0.0000 0.7854
+0.0000 0.0000 0.7854
+0.0000 0.0000 0.7854
+0.0000 0.0000 0.7854
+0.0000 0.0000 0.7854
+0.0000 0.0000 0.7854
+primID: 8
+startangle_c: 2
+endpose_c: 0 0 2
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 0.7854
+0.0000 0.0000 0.7854
+0.0000 0.0000 0.7854
+0.0000 0.0000 0.7854
+0.0000 0.0000 0.7854
+0.0000 0.0000 0.7854
+0.0000 0.0000 0.7854
+0.0000 0.0000 0.7854
+0.0000 0.0000 0.7854
+0.0000 0.0000 0.7854
+primID: 9
+startangle_c: 2
+endpose_c: 0 0 2
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 0.7854
+0.0000 0.0000 0.7854
+0.0000 0.0000 0.7854
+0.0000 0.0000 0.7854
+0.0000 0.0000 0.7854
+0.0000 0.0000 0.7854
+0.0000 0.0000 0.7854
+0.0000 0.0000 0.7854
+0.0000 0.0000 0.7854
+0.0000 0.0000 0.7854
+primID: 0
+startangle_c: 3
+endpose_c: 1 2 3
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 1.1781
+0.0056 0.0111 1.1781
+0.0111 0.0222 1.1781
+0.0167 0.0333 1.1781
+0.0222 0.0444 1.1781
+0.0278 0.0556 1.1781
+0.0333 0.0667 1.1781
+0.0389 0.0778 1.1781
+0.0444 0.0889 1.1781
+0.0500 0.1000 1.1781
+primID: 1
+startangle_c: 3
+endpose_c: 3 6 3
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 1.1781
+0.0167 0.0333 1.1781
+0.0333 0.0667 1.1781
+0.0500 0.1000 1.1781
+0.0667 0.1333 1.1781
+0.0833 0.1667 1.1781
+0.1000 0.2000 1.1781
+0.1167 0.2333 1.1781
+0.1333 0.2667 1.1781
+0.1500 0.3000 1.1781
+primID: 2
+startangle_c: 3
+endpose_c: -1 -2 3
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 1.1781
+-0.0056 -0.0111 1.1781
+-0.0111 -0.0222 1.1781
+-0.0167 -0.0333 1.1781
+-0.0222 -0.0444 1.1781
+-0.0278 -0.0556 1.1781
+-0.0333 -0.0667 1.1781
+-0.0389 -0.0778 1.1781
+-0.0444 -0.0889 1.1781
+-0.0500 -0.1000 1.1781
+primID: 3
+startangle_c: 3
+endpose_c: 4 5 2
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 1.1781
+0.0184 0.0298 1.1478
+0.0379 0.0592 1.1175
+0.0583 0.0881 1.0872
+0.0796 0.1165 1.0569
+0.1019 0.1444 1.0266
+0.1251 0.1717 0.9963
+0.1492 0.1984 0.9660
+0.1742 0.2245 0.9357
+0.2000 0.2500 0.9054
+primID: 4
+startangle_c: 3
+endpose_c: 2 7 4
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 1.1781
+0.0156 0.0377 1.1781
+0.0312 0.0754 1.1781
+0.0468 0.1131 1.1781
+0.0623 0.1508 1.1972
+0.0758 0.1893 1.2719
+0.0863 0.2287 1.3466
+0.0939 0.2687 1.4214
+0.0985 0.3092 1.4961
+0.1000 0.3500 1.5708
+primID: 5
+startangle_c: 3
+endpose_c: 0 0 2
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 1.1781
+0.0000 0.0000 1.1345
+0.0000 0.0000 1.0908
+0.0000 0.0000 1.0472
+0.0000 0.0000 1.0036
+0.0000 0.0000 0.9599
+0.0000 0.0000 0.9163
+0.0000 0.0000 0.8727
+0.0000 0.0000 0.8290
+0.0000 0.0000 0.7854
+primID: 6
+startangle_c: 3
+endpose_c: 0 0 4
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 1.1781
+0.0000 0.0000 1.2217
+0.0000 0.0000 1.2654
+0.0000 0.0000 1.3090
+0.0000 0.0000 1.3526
+0.0000 0.0000 1.3963
+0.0000 0.0000 1.4399
+0.0000 0.0000 1.4835
+0.0000 0.0000 1.5272
+0.0000 0.0000 1.5708
+primID: 7
+startangle_c: 3
+endpose_c: 0 0 3
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 1.1781
+0.0000 0.0000 1.1781
+0.0000 0.0000 1.1781
+0.0000 0.0000 1.1781
+0.0000 0.0000 1.1781
+0.0000 0.0000 1.1781
+0.0000 0.0000 1.1781
+0.0000 0.0000 1.1781
+0.0000 0.0000 1.1781
+0.0000 0.0000 1.1781
+primID: 8
+startangle_c: 3
+endpose_c: 0 0 3
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 1.1781
+0.0000 0.0000 1.1781
+0.0000 0.0000 1.1781
+0.0000 0.0000 1.1781
+0.0000 0.0000 1.1781
+0.0000 0.0000 1.1781
+0.0000 0.0000 1.1781
+0.0000 0.0000 1.1781
+0.0000 0.0000 1.1781
+0.0000 0.0000 1.1781
+primID: 9
+startangle_c: 3
+endpose_c: 0 0 3
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 1.1781
+0.0000 0.0000 1.1781
+0.0000 0.0000 1.1781
+0.0000 0.0000 1.1781
+0.0000 0.0000 1.1781
+0.0000 0.0000 1.1781
+0.0000 0.0000 1.1781
+0.0000 0.0000 1.1781
+0.0000 0.0000 1.1781
+0.0000 0.0000 1.1781
+primID: 0
+startangle_c: 4
+endpose_c: 0 1 4
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 1.5708
+0.0000 0.0056 1.5708
+0.0000 0.0111 1.5708
+0.0000 0.0167 1.5708
+0.0000 0.0222 1.5708
+0.0000 0.0278 1.5708
+0.0000 0.0333 1.5708
+0.0000 0.0389 1.5708
+0.0000 0.0444 1.5708
+0.0000 0.0500 1.5708
+primID: 1
+startangle_c: 4
+endpose_c: 0 8 4
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 1.5708
+0.0000 0.0444 1.5708
+0.0000 0.0889 1.5708
+0.0000 0.1333 1.5708
+0.0000 0.1778 1.5708
+0.0000 0.2222 1.5708
+0.0000 0.2667 1.5708
+0.0000 0.3111 1.5708
+0.0000 0.3556 1.5708
+0.0000 0.4000 1.5708
+primID: 2
+startangle_c: 4
+endpose_c: 0 -1 4
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 1.5708
+0.0000 -0.0056 1.5708
+0.0000 -0.0111 1.5708
+0.0000 -0.0167 1.5708
+0.0000 -0.0222 1.5708
+0.0000 -0.0278 1.5708
+0.0000 -0.0333 1.5708
+0.0000 -0.0389 1.5708
+0.0000 -0.0444 1.5708
+0.0000 -0.0500 1.5708
+primID: 3
+startangle_c: 4
+endpose_c: -1 8 5
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 1.5708
+0.0000 0.0452 1.5708
+0.0000 0.0904 1.5708
+0.0000 0.1355 1.5708
+-0.0008 0.1807 1.6196
+-0.0045 0.2257 1.6884
+-0.0114 0.2703 1.7572
+-0.0213 0.3144 1.8259
+-0.0342 0.3577 1.8947
+-0.0500 0.4000 1.9635
+primID: 4
+startangle_c: 4
+endpose_c: 1 8 3
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 1.5708
+0.0000 0.0452 1.5708
+0.0000 0.0904 1.5708
+0.0000 0.1355 1.5708
+0.0008 0.1807 1.5220
+0.0045 0.2257 1.4532
+0.0114 0.2703 1.3844
+0.0213 0.3144 1.3156
+0.0342 0.3577 1.2469
+0.0500 0.4000 1.1781
+primID: 5
+startangle_c: 4
+endpose_c: 0 0 5
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 1.5708
+0.0000 0.0000 1.6144
+0.0000 0.0000 1.6581
+0.0000 0.0000 1.7017
+0.0000 0.0000 1.7453
+0.0000 0.0000 1.7890
+0.0000 0.0000 1.8326
+0.0000 0.0000 1.8762
+0.0000 0.0000 1.9199
+0.0000 0.0000 1.9635
+primID: 6
+startangle_c: 4
+endpose_c: 0 0 3
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 1.5708
+0.0000 0.0000 1.5272
+0.0000 0.0000 1.4835
+0.0000 0.0000 1.4399
+0.0000 0.0000 1.3963
+0.0000 0.0000 1.3526
+0.0000 0.0000 1.3090
+0.0000 0.0000 1.2654
+0.0000 0.0000 1.2217
+0.0000 0.0000 1.1781
+primID: 7
+startangle_c: 4
+endpose_c: 0 0 4
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 1.5708
+0.0000 0.0000 1.5708
+0.0000 0.0000 1.5708
+0.0000 0.0000 1.5708
+0.0000 0.0000 1.5708
+0.0000 0.0000 1.5708
+0.0000 0.0000 1.5708
+0.0000 0.0000 1.5708
+0.0000 0.0000 1.5708
+0.0000 0.0000 1.5708
+primID: 8
+startangle_c: 4
+endpose_c: 0 0 4
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 1.5708
+0.0000 0.0000 1.5708
+0.0000 0.0000 1.5708
+0.0000 0.0000 1.5708
+0.0000 0.0000 1.5708
+0.0000 0.0000 1.5708
+0.0000 0.0000 1.5708
+0.0000 0.0000 1.5708
+0.0000 0.0000 1.5708
+0.0000 0.0000 1.5708
+primID: 9
+startangle_c: 4
+endpose_c: 0 0 4
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 1.5708
+0.0000 0.0000 1.5708
+0.0000 0.0000 1.5708
+0.0000 0.0000 1.5708
+0.0000 0.0000 1.5708
+0.0000 0.0000 1.5708
+0.0000 0.0000 1.5708
+0.0000 0.0000 1.5708
+0.0000 0.0000 1.5708
+0.0000 0.0000 1.5708
+primID: 0
+startangle_c: 5
+endpose_c: -1 2 5
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 1.9635
+-0.0056 0.0111 1.9635
+-0.0111 0.0222 1.9635
+-0.0167 0.0333 1.9635
+-0.0222 0.0444 1.9635
+-0.0278 0.0556 1.9635
+-0.0333 0.0667 1.9635
+-0.0389 0.0778 1.9635
+-0.0444 0.0889 1.9635
+-0.0500 0.1000 1.9635
+primID: 1
+startangle_c: 5
+endpose_c: -3 6 5
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 1.9635
+-0.0167 0.0333 1.9635
+-0.0333 0.0667 1.9635
+-0.0500 0.1000 1.9635
+-0.0667 0.1333 1.9635
+-0.0833 0.1667 1.9635
+-0.1000 0.2000 1.9635
+-0.1167 0.2333 1.9635
+-0.1333 0.2667 1.9635
+-0.1500 0.3000 1.9635
+primID: 2
+startangle_c: 5
+endpose_c: 1 -2 5
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 1.9635
+0.0056 -0.0111 1.9635
+0.0111 -0.0222 1.9635
+0.0167 -0.0333 1.9635
+0.0222 -0.0444 1.9635
+0.0278 -0.0556 1.9635
+0.0333 -0.0667 1.9635
+0.0389 -0.0778 1.9635
+0.0444 -0.0889 1.9635
+0.0500 -0.1000 1.9635
+primID: 3
+startangle_c: 5
+endpose_c: -4 5 6
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 1.9635
+-0.0184 0.0298 1.9938
+-0.0379 0.0592 2.0241
+-0.0583 0.0881 2.0544
+-0.0796 0.1165 2.0847
+-0.1019 0.1444 2.1150
+-0.1251 0.1717 2.1453
+-0.1492 0.1984 2.1756
+-0.1742 0.2245 2.2059
+-0.2000 0.2500 2.2362
+primID: 4
+startangle_c: 5
+endpose_c: -2 7 4
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 1.9635
+-0.0156 0.0377 1.9635
+-0.0312 0.0754 1.9635
+-0.0468 0.1131 1.9635
+-0.0623 0.1508 1.9444
+-0.0758 0.1893 1.8697
+-0.0863 0.2287 1.7950
+-0.0939 0.2687 1.7202
+-0.0985 0.3092 1.6455
+-0.1000 0.3500 1.5708
+primID: 5
+startangle_c: 5
+endpose_c: 0 0 6
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 1.9635
+0.0000 0.0000 2.0071
+0.0000 0.0000 2.0508
+0.0000 0.0000 2.0944
+0.0000 0.0000 2.1380
+0.0000 0.0000 2.1817
+0.0000 0.0000 2.2253
+0.0000 0.0000 2.2689
+0.0000 0.0000 2.3126
+0.0000 0.0000 2.3562
+primID: 6
+startangle_c: 5
+endpose_c: 0 0 4
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 1.9635
+0.0000 0.0000 1.9199
+0.0000 0.0000 1.8762
+0.0000 0.0000 1.8326
+0.0000 0.0000 1.7890
+0.0000 0.0000 1.7453
+0.0000 0.0000 1.7017
+0.0000 0.0000 1.6581
+0.0000 0.0000 1.6144
+0.0000 0.0000 1.5708
+primID: 7
+startangle_c: 5
+endpose_c: 0 0 5
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 1.9635
+0.0000 0.0000 1.9635
+0.0000 0.0000 1.9635
+0.0000 0.0000 1.9635
+0.0000 0.0000 1.9635
+0.0000 0.0000 1.9635
+0.0000 0.0000 1.9635
+0.0000 0.0000 1.9635
+0.0000 0.0000 1.9635
+0.0000 0.0000 1.9635
+primID: 8
+startangle_c: 5
+endpose_c: 0 0 5
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 1.9635
+0.0000 0.0000 1.9635
+0.0000 0.0000 1.9635
+0.0000 0.0000 1.9635
+0.0000 0.0000 1.9635
+0.0000 0.0000 1.9635
+0.0000 0.0000 1.9635
+0.0000 0.0000 1.9635
+0.0000 0.0000 1.9635
+0.0000 0.0000 1.9635
+primID: 9
+startangle_c: 5
+endpose_c: 0 0 5
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 1.9635
+0.0000 0.0000 1.9635
+0.0000 0.0000 1.9635
+0.0000 0.0000 1.9635
+0.0000 0.0000 1.9635
+0.0000 0.0000 1.9635
+0.0000 0.0000 1.9635
+0.0000 0.0000 1.9635
+0.0000 0.0000 1.9635
+0.0000 0.0000 1.9635
+primID: 0
+startangle_c: 6
+endpose_c: -1 1 6
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 2.3562
+-0.0056 0.0056 2.3562
+-0.0111 0.0111 2.3562
+-0.0167 0.0167 2.3562
+-0.0222 0.0222 2.3562
+-0.0278 0.0278 2.3562
+-0.0333 0.0333 2.3562
+-0.0389 0.0389 2.3562
+-0.0444 0.0444 2.3562
+-0.0500 0.0500 2.3562
+primID: 1
+startangle_c: 6
+endpose_c: -6 6 6
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 2.3562
+-0.0333 0.0333 2.3562
+-0.0667 0.0667 2.3562
+-0.1000 0.1000 2.3562
+-0.1333 0.1333 2.3562
+-0.1667 0.1667 2.3562
+-0.2000 0.2000 2.3562
+-0.2333 0.2333 2.3562
+-0.2667 0.2667 2.3562
+-0.3000 0.3000 2.3562
+primID: 2
+startangle_c: 6
+endpose_c: 1 -1 6
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 2.3562
+0.0056 -0.0056 2.3562
+0.0111 -0.0111 2.3562
+0.0167 -0.0167 2.3562
+0.0222 -0.0222 2.3562
+0.0278 -0.0278 2.3562
+0.0333 -0.0333 2.3562
+0.0389 -0.0389 2.3562
+0.0444 -0.0444 2.3562
+0.0500 -0.0500 2.3562
+primID: 3
+startangle_c: 6
+endpose_c: -7 5 7
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 2.3562
+-0.0341 0.0341 2.3562
+-0.0684 0.0678 2.3859
+-0.1043 0.1000 2.4377
+-0.1418 0.1302 2.4896
+-0.1809 0.1584 2.5415
+-0.2213 0.1846 2.5933
+-0.2631 0.2086 2.6452
+-0.3060 0.2304 2.6970
+-0.3500 0.2500 2.7489
+primID: 4
+startangle_c: 6
+endpose_c: -5 7 5
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 2.3562
+-0.0341 0.0341 2.3562
+-0.0678 0.0684 2.3265
+-0.1000 0.1043 2.2747
+-0.1302 0.1418 2.2228
+-0.1584 0.1809 2.1709
+-0.1846 0.2213 2.1191
+-0.2086 0.2631 2.0672
+-0.2304 0.3060 2.0154
+-0.2500 0.3500 1.9635
+primID: 5
+startangle_c: 6
+endpose_c: 0 0 7
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 2.3562
+0.0000 0.0000 2.3998
+0.0000 0.0000 2.4435
+0.0000 0.0000 2.4871
+0.0000 0.0000 2.5307
+0.0000 0.0000 2.5744
+0.0000 0.0000 2.6180
+0.0000 0.0000 2.6616
+0.0000 0.0000 2.7053
+0.0000 0.0000 2.7489
+primID: 6
+startangle_c: 6
+endpose_c: 0 0 5
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 2.3562
+0.0000 0.0000 2.3126
+0.0000 0.0000 2.2689
+0.0000 0.0000 2.2253
+0.0000 0.0000 2.1817
+0.0000 0.0000 2.1380
+0.0000 0.0000 2.0944
+0.0000 0.0000 2.0508
+0.0000 0.0000 2.0071
+0.0000 0.0000 1.9635
+primID: 7
+startangle_c: 6
+endpose_c: 0 0 6
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 2.3562
+0.0000 0.0000 2.3562
+0.0000 0.0000 2.3562
+0.0000 0.0000 2.3562
+0.0000 0.0000 2.3562
+0.0000 0.0000 2.3562
+0.0000 0.0000 2.3562
+0.0000 0.0000 2.3562
+0.0000 0.0000 2.3562
+0.0000 0.0000 2.3562
+primID: 8
+startangle_c: 6
+endpose_c: 0 0 6
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 2.3562
+0.0000 0.0000 2.3562
+0.0000 0.0000 2.3562
+0.0000 0.0000 2.3562
+0.0000 0.0000 2.3562
+0.0000 0.0000 2.3562
+0.0000 0.0000 2.3562
+0.0000 0.0000 2.3562
+0.0000 0.0000 2.3562
+0.0000 0.0000 2.3562
+primID: 9
+startangle_c: 6
+endpose_c: 0 0 6
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 2.3562
+0.0000 0.0000 2.3562
+0.0000 0.0000 2.3562
+0.0000 0.0000 2.3562
+0.0000 0.0000 2.3562
+0.0000 0.0000 2.3562
+0.0000 0.0000 2.3562
+0.0000 0.0000 2.3562
+0.0000 0.0000 2.3562
+0.0000 0.0000 2.3562
+primID: 0
+startangle_c: 7
+endpose_c: -2 1 7
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 2.7489
+-0.0111 0.0056 2.7489
+-0.0222 0.0111 2.7489
+-0.0333 0.0167 2.7489
+-0.0444 0.0222 2.7489
+-0.0556 0.0278 2.7489
+-0.0667 0.0333 2.7489
+-0.0778 0.0389 2.7489
+-0.0889 0.0444 2.7489
+-0.1000 0.0500 2.7489
+primID: 1
+startangle_c: 7
+endpose_c: -6 3 7
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 2.7489
+-0.0333 0.0167 2.7489
+-0.0667 0.0333 2.7489
+-0.1000 0.0500 2.7489
+-0.1333 0.0667 2.7489
+-0.1667 0.0833 2.7489
+-0.2000 0.1000 2.7489
+-0.2333 0.1167 2.7489
+-0.2667 0.1333 2.7489
+-0.3000 0.1500 2.7489
+primID: 2
+startangle_c: 7
+endpose_c: 2 -1 7
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 2.7489
+0.0111 -0.0056 2.7489
+0.0222 -0.0111 2.7489
+0.0333 -0.0167 2.7489
+0.0444 -0.0222 2.7489
+0.0556 -0.0278 2.7489
+0.0667 -0.0333 2.7489
+0.0778 -0.0389 2.7489
+0.0889 -0.0444 2.7489
+0.1000 -0.0500 2.7489
+primID: 3
+startangle_c: 7
+endpose_c: -5 4 6
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 2.7489
+-0.0298 0.0184 2.7186
+-0.0592 0.0379 2.6883
+-0.0881 0.0583 2.6580
+-0.1165 0.0796 2.6277
+-0.1444 0.1019 2.5974
+-0.1717 0.1251 2.5671
+-0.1984 0.1492 2.5368
+-0.2245 0.1742 2.5065
+-0.2500 0.2000 2.4762
+primID: 4
+startangle_c: 7
+endpose_c: -7 2 8
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 2.7489
+-0.0377 0.0156 2.7489
+-0.0754 0.0312 2.7489
+-0.1131 0.0468 2.7489
+-0.1508 0.0623 2.7680
+-0.1893 0.0758 2.8427
+-0.2287 0.0863 2.9174
+-0.2687 0.0939 2.9921
+-0.3092 0.0985 3.0669
+-0.3500 0.1000 3.1416
+primID: 5
+startangle_c: 7
+endpose_c: 0 0 6
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 2.7489
+0.0000 0.0000 2.7053
+0.0000 0.0000 2.6616
+0.0000 0.0000 2.6180
+0.0000 0.0000 2.5744
+0.0000 0.0000 2.5307
+0.0000 0.0000 2.4871
+0.0000 0.0000 2.4435
+0.0000 0.0000 2.3998
+0.0000 0.0000 2.3562
+primID: 6
+startangle_c: 7
+endpose_c: 0 0 8
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 2.7489
+0.0000 0.0000 2.7925
+0.0000 0.0000 2.8362
+0.0000 0.0000 2.8798
+0.0000 0.0000 2.9234
+0.0000 0.0000 2.9671
+0.0000 0.0000 3.0107
+0.0000 0.0000 3.0543
+0.0000 0.0000 3.0980
+0.0000 0.0000 3.1416
+primID: 7
+startangle_c: 7
+endpose_c: 0 0 7
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 2.7489
+0.0000 0.0000 2.7489
+0.0000 0.0000 2.7489
+0.0000 0.0000 2.7489
+0.0000 0.0000 2.7489
+0.0000 0.0000 2.7489
+0.0000 0.0000 2.7489
+0.0000 0.0000 2.7489
+0.0000 0.0000 2.7489
+0.0000 0.0000 2.7489
+primID: 8
+startangle_c: 7
+endpose_c: 0 0 7
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 2.7489
+0.0000 0.0000 2.7489
+0.0000 0.0000 2.7489
+0.0000 0.0000 2.7489
+0.0000 0.0000 2.7489
+0.0000 0.0000 2.7489
+0.0000 0.0000 2.7489
+0.0000 0.0000 2.7489
+0.0000 0.0000 2.7489
+0.0000 0.0000 2.7489
+primID: 9
+startangle_c: 7
+endpose_c: 0 0 7
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 2.7489
+0.0000 0.0000 2.7489
+0.0000 0.0000 2.7489
+0.0000 0.0000 2.7489
+0.0000 0.0000 2.7489
+0.0000 0.0000 2.7489
+0.0000 0.0000 2.7489
+0.0000 0.0000 2.7489
+0.0000 0.0000 2.7489
+0.0000 0.0000 2.7489
+primID: 0
+startangle_c: 8
+endpose_c: -1 0 8
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 3.1416
+-0.0056 0.0000 3.1416
+-0.0111 0.0000 3.1416
+-0.0167 0.0000 3.1416
+-0.0222 0.0000 3.1416
+-0.0278 0.0000 3.1416
+-0.0333 0.0000 3.1416
+-0.0389 0.0000 3.1416
+-0.0444 0.0000 3.1416
+-0.0500 0.0000 3.1416
+primID: 1
+startangle_c: 8
+endpose_c: -8 0 8
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 3.1416
+-0.0444 0.0000 3.1416
+-0.0889 0.0000 3.1416
+-0.1333 0.0000 3.1416
+-0.1778 0.0000 3.1416
+-0.2222 0.0000 3.1416
+-0.2667 0.0000 3.1416
+-0.3111 0.0000 3.1416
+-0.3556 0.0000 3.1416
+-0.4000 0.0000 3.1416
+primID: 2
+startangle_c: 8
+endpose_c: 1 0 8
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 3.1416
+0.0056 0.0000 3.1416
+0.0111 0.0000 3.1416
+0.0167 0.0000 3.1416
+0.0222 0.0000 3.1416
+0.0278 0.0000 3.1416
+0.0333 0.0000 3.1416
+0.0389 0.0000 3.1416
+0.0444 0.0000 3.1416
+0.0500 0.0000 3.1416
+primID: 3
+startangle_c: 8
+endpose_c: -8 -1 9
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 3.1416
+-0.0452 0.0000 3.1416
+-0.0904 0.0000 3.1416
+-0.1355 0.0000 3.1416
+-0.1807 -0.0008 3.1904
+-0.2257 -0.0045 3.2592
+-0.2703 -0.0114 3.3280
+-0.3144 -0.0213 3.3967
+-0.3577 -0.0342 3.4655
+-0.4000 -0.0500 3.5343
+primID: 4
+startangle_c: 8
+endpose_c: -8 1 7
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 3.1416
+-0.0452 0.0000 3.1416
+-0.0904 0.0000 3.1416
+-0.1355 0.0000 3.1416
+-0.1807 0.0008 3.0928
+-0.2257 0.0045 3.0240
+-0.2703 0.0114 2.9552
+-0.3144 0.0213 2.8864
+-0.3577 0.0342 2.8177
+-0.4000 0.0500 2.7489
+primID: 5
+startangle_c: 8
+endpose_c: 0 0 9
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 3.1416
+0.0000 0.0000 3.1852
+0.0000 0.0000 3.2289
+0.0000 0.0000 3.2725
+0.0000 0.0000 3.3161
+0.0000 0.0000 3.3598
+0.0000 0.0000 3.4034
+0.0000 0.0000 3.4470
+0.0000 0.0000 3.4907
+0.0000 0.0000 3.5343
+primID: 6
+startangle_c: 8
+endpose_c: 0 0 7
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 3.1416
+0.0000 0.0000 3.0980
+0.0000 0.0000 3.0543
+0.0000 0.0000 3.0107
+0.0000 0.0000 2.9671
+0.0000 0.0000 2.9234
+0.0000 0.0000 2.8798
+0.0000 0.0000 2.8362
+0.0000 0.0000 2.7925
+0.0000 0.0000 2.7489
+primID: 7
+startangle_c: 8
+endpose_c: 0 0 8
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 3.1416
+0.0000 0.0000 3.1416
+0.0000 0.0000 3.1416
+0.0000 0.0000 3.1416
+0.0000 0.0000 3.1416
+0.0000 0.0000 3.1416
+0.0000 0.0000 3.1416
+0.0000 0.0000 3.1416
+0.0000 0.0000 3.1416
+0.0000 0.0000 3.1416
+primID: 8
+startangle_c: 8
+endpose_c: 0 0 8
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 3.1416
+0.0000 0.0000 3.1416
+0.0000 0.0000 3.1416
+0.0000 0.0000 3.1416
+0.0000 0.0000 3.1416
+0.0000 0.0000 3.1416
+0.0000 0.0000 3.1416
+0.0000 0.0000 3.1416
+0.0000 0.0000 3.1416
+0.0000 0.0000 3.1416
+primID: 9
+startangle_c: 8
+endpose_c: 0 0 8
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 3.1416
+0.0000 0.0000 3.1416
+0.0000 0.0000 3.1416
+0.0000 0.0000 3.1416
+0.0000 0.0000 3.1416
+0.0000 0.0000 3.1416
+0.0000 0.0000 3.1416
+0.0000 0.0000 3.1416
+0.0000 0.0000 3.1416
+0.0000 0.0000 3.1416
+primID: 0
+startangle_c: 9
+endpose_c: -2 -1 9
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 3.5343
+-0.0111 -0.0056 3.5343
+-0.0222 -0.0111 3.5343
+-0.0333 -0.0167 3.5343
+-0.0444 -0.0222 3.5343
+-0.0556 -0.0278 3.5343
+-0.0667 -0.0333 3.5343
+-0.0778 -0.0389 3.5343
+-0.0889 -0.0444 3.5343
+-0.1000 -0.0500 3.5343
+primID: 1
+startangle_c: 9
+endpose_c: -6 -3 9
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 3.5343
+-0.0333 -0.0167 3.5343
+-0.0667 -0.0333 3.5343
+-0.1000 -0.0500 3.5343
+-0.1333 -0.0667 3.5343
+-0.1667 -0.0833 3.5343
+-0.2000 -0.1000 3.5343
+-0.2333 -0.1167 3.5343
+-0.2667 -0.1333 3.5343
+-0.3000 -0.1500 3.5343
+primID: 2
+startangle_c: 9
+endpose_c: 2 1 9
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 3.5343
+0.0111 0.0056 3.5343
+0.0222 0.0111 3.5343
+0.0333 0.0167 3.5343
+0.0444 0.0222 3.5343
+0.0556 0.0278 3.5343
+0.0667 0.0333 3.5343
+0.0778 0.0389 3.5343
+0.0889 0.0444 3.5343
+0.1000 0.0500 3.5343
+primID: 3
+startangle_c: 9
+endpose_c: -5 -4 10
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 3.5343
+-0.0298 -0.0184 3.5646
+-0.0592 -0.0379 3.5949
+-0.0881 -0.0583 3.6252
+-0.1165 -0.0796 3.6555
+-0.1444 -0.1019 3.6858
+-0.1717 -0.1251 3.7161
+-0.1984 -0.1492 3.7464
+-0.2245 -0.1742 3.7767
+-0.2500 -0.2000 3.8070
+primID: 4
+startangle_c: 9
+endpose_c: -7 -2 8
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 3.5343
+-0.0377 -0.0156 3.5343
+-0.0754 -0.0312 3.5343
+-0.1131 -0.0468 3.5343
+-0.1508 -0.0623 3.5152
+-0.1893 -0.0758 3.4405
+-0.2287 -0.0863 3.3658
+-0.2687 -0.0939 3.2910
+-0.3092 -0.0985 3.2163
+-0.3500 -0.1000 3.1416
+primID: 5
+startangle_c: 9
+endpose_c: 0 0 10
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 3.5343
+0.0000 0.0000 3.5779
+0.0000 0.0000 3.6216
+0.0000 0.0000 3.6652
+0.0000 0.0000 3.7088
+0.0000 0.0000 3.7525
+0.0000 0.0000 3.7961
+0.0000 0.0000 3.8397
+0.0000 0.0000 3.8834
+0.0000 0.0000 3.9270
+primID: 6
+startangle_c: 9
+endpose_c: 0 0 8
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 3.5343
+0.0000 0.0000 3.4907
+0.0000 0.0000 3.4470
+0.0000 0.0000 3.4034
+0.0000 0.0000 3.3598
+0.0000 0.0000 3.3161
+0.0000 0.0000 3.2725
+0.0000 0.0000 3.2289
+0.0000 0.0000 3.1852
+0.0000 0.0000 3.1416
+primID: 7
+startangle_c: 9
+endpose_c: 0 0 9
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 3.5343
+0.0000 0.0000 3.5343
+0.0000 0.0000 3.5343
+0.0000 0.0000 3.5343
+0.0000 0.0000 3.5343
+0.0000 0.0000 3.5343
+0.0000 0.0000 3.5343
+0.0000 0.0000 3.5343
+0.0000 0.0000 3.5343
+0.0000 0.0000 3.5343
+primID: 8
+startangle_c: 9
+endpose_c: 0 0 9
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 3.5343
+0.0000 0.0000 3.5343
+0.0000 0.0000 3.5343
+0.0000 0.0000 3.5343
+0.0000 0.0000 3.5343
+0.0000 0.0000 3.5343
+0.0000 0.0000 3.5343
+0.0000 0.0000 3.5343
+0.0000 0.0000 3.5343
+0.0000 0.0000 3.5343
+primID: 9
+startangle_c: 9
+endpose_c: 0 0 9
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 3.5343
+0.0000 0.0000 3.5343
+0.0000 0.0000 3.5343
+0.0000 0.0000 3.5343
+0.0000 0.0000 3.5343
+0.0000 0.0000 3.5343
+0.0000 0.0000 3.5343
+0.0000 0.0000 3.5343
+0.0000 0.0000 3.5343
+0.0000 0.0000 3.5343
+primID: 0
+startangle_c: 10
+endpose_c: -1 -1 10
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 3.9270
+-0.0056 -0.0056 3.9270
+-0.0111 -0.0111 3.9270
+-0.0167 -0.0167 3.9270
+-0.0222 -0.0222 3.9270
+-0.0278 -0.0278 3.9270
+-0.0333 -0.0333 3.9270
+-0.0389 -0.0389 3.9270
+-0.0444 -0.0444 3.9270
+-0.0500 -0.0500 3.9270
+primID: 1
+startangle_c: 10
+endpose_c: -6 -6 10
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 3.9270
+-0.0333 -0.0333 3.9270
+-0.0667 -0.0667 3.9270
+-0.1000 -0.1000 3.9270
+-0.1333 -0.1333 3.9270
+-0.1667 -0.1667 3.9270
+-0.2000 -0.2000 3.9270
+-0.2333 -0.2333 3.9270
+-0.2667 -0.2667 3.9270
+-0.3000 -0.3000 3.9270
+primID: 2
+startangle_c: 10
+endpose_c: 1 1 10
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 3.9270
+0.0056 0.0056 3.9270
+0.0111 0.0111 3.9270
+0.0167 0.0167 3.9270
+0.0222 0.0222 3.9270
+0.0278 0.0278 3.9270
+0.0333 0.0333 3.9270
+0.0389 0.0389 3.9270
+0.0444 0.0444 3.9270
+0.0500 0.0500 3.9270
+primID: 3
+startangle_c: 10
+endpose_c: -5 -7 11
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 3.9270
+-0.0341 -0.0341 3.9270
+-0.0678 -0.0684 3.9567
+-0.1000 -0.1043 4.0085
+-0.1302 -0.1418 4.0604
+-0.1584 -0.1809 4.1123
+-0.1846 -0.2213 4.1641
+-0.2086 -0.2631 4.2160
+-0.2304 -0.3060 4.2678
+-0.2500 -0.3500 4.3197
+primID: 4
+startangle_c: 10
+endpose_c: -7 -5 9
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 3.9270
+-0.0341 -0.0341 3.9270
+-0.0684 -0.0678 3.8973
+-0.1043 -0.1000 3.8455
+-0.1418 -0.1302 3.7936
+-0.1809 -0.1584 3.7417
+-0.2213 -0.1846 3.6899
+-0.2631 -0.2086 3.6380
+-0.3060 -0.2304 3.5862
+-0.3500 -0.2500 3.5343
+primID: 5
+startangle_c: 10
+endpose_c: 0 0 11
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 3.9270
+0.0000 0.0000 3.9706
+0.0000 0.0000 4.0143
+0.0000 0.0000 4.0579
+0.0000 0.0000 4.1015
+0.0000 0.0000 4.1452
+0.0000 0.0000 4.1888
+0.0000 0.0000 4.2324
+0.0000 0.0000 4.2761
+0.0000 0.0000 4.3197
+primID: 6
+startangle_c: 10
+endpose_c: 0 0 9
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 3.9270
+0.0000 0.0000 3.8834
+0.0000 0.0000 3.8397
+0.0000 0.0000 3.7961
+0.0000 0.0000 3.7525
+0.0000 0.0000 3.7088
+0.0000 0.0000 3.6652
+0.0000 0.0000 3.6216
+0.0000 0.0000 3.5779
+0.0000 0.0000 3.5343
+primID: 7
+startangle_c: 10
+endpose_c: 0 0 10
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 3.9270
+0.0000 0.0000 3.9270
+0.0000 0.0000 3.9270
+0.0000 0.0000 3.9270
+0.0000 0.0000 3.9270
+0.0000 0.0000 3.9270
+0.0000 0.0000 3.9270
+0.0000 0.0000 3.9270
+0.0000 0.0000 3.9270
+0.0000 0.0000 3.9270
+primID: 8
+startangle_c: 10
+endpose_c: 0 0 10
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 3.9270
+0.0000 0.0000 3.9270
+0.0000 0.0000 3.9270
+0.0000 0.0000 3.9270
+0.0000 0.0000 3.9270
+0.0000 0.0000 3.9270
+0.0000 0.0000 3.9270
+0.0000 0.0000 3.9270
+0.0000 0.0000 3.9270
+0.0000 0.0000 3.9270
+primID: 9
+startangle_c: 10
+endpose_c: 0 0 10
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 3.9270
+0.0000 0.0000 3.9270
+0.0000 0.0000 3.9270
+0.0000 0.0000 3.9270
+0.0000 0.0000 3.9270
+0.0000 0.0000 3.9270
+0.0000 0.0000 3.9270
+0.0000 0.0000 3.9270
+0.0000 0.0000 3.9270
+0.0000 0.0000 3.9270
+primID: 0
+startangle_c: 11
+endpose_c: -1 -2 11
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 4.3197
+-0.0056 -0.0111 4.3197
+-0.0111 -0.0222 4.3197
+-0.0167 -0.0333 4.3197
+-0.0222 -0.0444 4.3197
+-0.0278 -0.0556 4.3197
+-0.0333 -0.0667 4.3197
+-0.0389 -0.0778 4.3197
+-0.0444 -0.0889 4.3197
+-0.0500 -0.1000 4.3197
+primID: 1
+startangle_c: 11
+endpose_c: -3 -6 11
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 4.3197
+-0.0167 -0.0333 4.3197
+-0.0333 -0.0667 4.3197
+-0.0500 -0.1000 4.3197
+-0.0667 -0.1333 4.3197
+-0.0833 -0.1667 4.3197
+-0.1000 -0.2000 4.3197
+-0.1167 -0.2333 4.3197
+-0.1333 -0.2667 4.3197
+-0.1500 -0.3000 4.3197
+primID: 2
+startangle_c: 11
+endpose_c: 1 2 11
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 4.3197
+0.0056 0.0111 4.3197
+0.0111 0.0222 4.3197
+0.0167 0.0333 4.3197
+0.0222 0.0444 4.3197
+0.0278 0.0556 4.3197
+0.0333 0.0667 4.3197
+0.0389 0.0778 4.3197
+0.0444 0.0889 4.3197
+0.0500 0.1000 4.3197
+primID: 3
+startangle_c: 11
+endpose_c: -4 -5 10
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 4.3197
+-0.0184 -0.0298 4.2894
+-0.0379 -0.0592 4.2591
+-0.0583 -0.0881 4.2288
+-0.0796 -0.1165 4.1985
+-0.1019 -0.1444 4.1682
+-0.1251 -0.1717 4.1379
+-0.1492 -0.1984 4.1076
+-0.1742 -0.2245 4.0773
+-0.2000 -0.2500 4.0470
+primID: 4
+startangle_c: 11
+endpose_c: -2 -7 12
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 4.3197
+-0.0156 -0.0377 4.3197
+-0.0312 -0.0754 4.3197
+-0.0468 -0.1131 4.3197
+-0.0623 -0.1508 4.3388
+-0.0758 -0.1893 4.4135
+-0.0863 -0.2287 4.4882
+-0.0939 -0.2687 4.5629
+-0.0985 -0.3092 4.6377
+-0.1000 -0.3500 4.7124
+primID: 5
+startangle_c: 11
+endpose_c: 0 0 10
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 4.3197
+0.0000 0.0000 4.2761
+0.0000 0.0000 4.2324
+0.0000 0.0000 4.1888
+0.0000 0.0000 4.1452
+0.0000 0.0000 4.1015
+0.0000 0.0000 4.0579
+0.0000 0.0000 4.0143
+0.0000 0.0000 3.9706
+0.0000 0.0000 3.9270
+primID: 6
+startangle_c: 11
+endpose_c: 0 0 12
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 4.3197
+0.0000 0.0000 4.3633
+0.0000 0.0000 4.4070
+0.0000 0.0000 4.4506
+0.0000 0.0000 4.4942
+0.0000 0.0000 4.5379
+0.0000 0.0000 4.5815
+0.0000 0.0000 4.6251
+0.0000 0.0000 4.6688
+0.0000 0.0000 4.7124
+primID: 7
+startangle_c: 11
+endpose_c: 0 0 11
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 4.3197
+0.0000 0.0000 4.3197
+0.0000 0.0000 4.3197
+0.0000 0.0000 4.3197
+0.0000 0.0000 4.3197
+0.0000 0.0000 4.3197
+0.0000 0.0000 4.3197
+0.0000 0.0000 4.3197
+0.0000 0.0000 4.3197
+0.0000 0.0000 4.3197
+primID: 8
+startangle_c: 11
+endpose_c: 0 0 11
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 4.3197
+0.0000 0.0000 4.3197
+0.0000 0.0000 4.3197
+0.0000 0.0000 4.3197
+0.0000 0.0000 4.3197
+0.0000 0.0000 4.3197
+0.0000 0.0000 4.3197
+0.0000 0.0000 4.3197
+0.0000 0.0000 4.3197
+0.0000 0.0000 4.3197
+primID: 9
+startangle_c: 11
+endpose_c: 0 0 11
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 4.3197
+0.0000 0.0000 4.3197
+0.0000 0.0000 4.3197
+0.0000 0.0000 4.3197
+0.0000 0.0000 4.3197
+0.0000 0.0000 4.3197
+0.0000 0.0000 4.3197
+0.0000 0.0000 4.3197
+0.0000 0.0000 4.3197
+0.0000 0.0000 4.3197
+primID: 0
+startangle_c: 12
+endpose_c: 0 -1 12
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 4.7124
+0.0000 -0.0056 4.7124
+0.0000 -0.0111 4.7124
+0.0000 -0.0167 4.7124
+0.0000 -0.0222 4.7124
+0.0000 -0.0278 4.7124
+0.0000 -0.0333 4.7124
+0.0000 -0.0389 4.7124
+0.0000 -0.0444 4.7124
+0.0000 -0.0500 4.7124
+primID: 1
+startangle_c: 12
+endpose_c: 0 -8 12
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 4.7124
+0.0000 -0.0444 4.7124
+0.0000 -0.0889 4.7124
+0.0000 -0.1333 4.7124
+0.0000 -0.1778 4.7124
+0.0000 -0.2222 4.7124
+0.0000 -0.2667 4.7124
+0.0000 -0.3111 4.7124
+0.0000 -0.3556 4.7124
+0.0000 -0.4000 4.7124
+primID: 2
+startangle_c: 12
+endpose_c: 0 1 12
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 4.7124
+0.0000 0.0056 4.7124
+0.0000 0.0111 4.7124
+0.0000 0.0167 4.7124
+0.0000 0.0222 4.7124
+0.0000 0.0278 4.7124
+0.0000 0.0333 4.7124
+0.0000 0.0389 4.7124
+0.0000 0.0444 4.7124
+0.0000 0.0500 4.7124
+primID: 3
+startangle_c: 12
+endpose_c: 1 -8 13
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 4.7124
+-0.0000 -0.0452 4.7124
+-0.0000 -0.0904 4.7124
+-0.0000 -0.1355 4.7124
+0.0008 -0.1807 4.7612
+0.0045 -0.2257 4.8300
+0.0114 -0.2703 4.8988
+0.0213 -0.3144 4.9675
+0.0342 -0.3577 5.0363
+0.0500 -0.4000 5.1051
+primID: 4
+startangle_c: 12
+endpose_c: -1 -8 11
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 4.7124
+-0.0000 -0.0452 4.7124
+-0.0000 -0.0904 4.7124
+-0.0000 -0.1355 4.7124
+-0.0008 -0.1807 4.6636
+-0.0045 -0.2257 4.5948
+-0.0114 -0.2703 4.5260
+-0.0213 -0.3144 4.4572
+-0.0342 -0.3577 4.3885
+-0.0500 -0.4000 4.3197
+primID: 5
+startangle_c: 12
+endpose_c: 0 0 13
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 4.7124
+0.0000 0.0000 4.7560
+0.0000 0.0000 4.7997
+0.0000 0.0000 4.8433
+0.0000 0.0000 4.8869
+0.0000 0.0000 4.9306
+0.0000 0.0000 4.9742
+0.0000 0.0000 5.0178
+0.0000 0.0000 5.0615
+0.0000 0.0000 5.1051
+primID: 6
+startangle_c: 12
+endpose_c: 0 0 11
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 4.7124
+0.0000 0.0000 4.6688
+0.0000 0.0000 4.6251
+0.0000 0.0000 4.5815
+0.0000 0.0000 4.5379
+0.0000 0.0000 4.4942
+0.0000 0.0000 4.4506
+0.0000 0.0000 4.4070
+0.0000 0.0000 4.3633
+0.0000 0.0000 4.3197
+primID: 7
+startangle_c: 12
+endpose_c: 0 0 12
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 4.7124
+0.0000 0.0000 4.7124
+0.0000 0.0000 4.7124
+0.0000 0.0000 4.7124
+0.0000 0.0000 4.7124
+0.0000 0.0000 4.7124
+0.0000 0.0000 4.7124
+0.0000 0.0000 4.7124
+0.0000 0.0000 4.7124
+0.0000 0.0000 4.7124
+primID: 8
+startangle_c: 12
+endpose_c: 0 0 12
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 4.7124
+0.0000 0.0000 4.7124
+0.0000 0.0000 4.7124
+0.0000 0.0000 4.7124
+0.0000 0.0000 4.7124
+0.0000 0.0000 4.7124
+0.0000 0.0000 4.7124
+0.0000 0.0000 4.7124
+0.0000 0.0000 4.7124
+0.0000 0.0000 4.7124
+primID: 9
+startangle_c: 12
+endpose_c: 0 0 12
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 4.7124
+0.0000 0.0000 4.7124
+0.0000 0.0000 4.7124
+0.0000 0.0000 4.7124
+0.0000 0.0000 4.7124
+0.0000 0.0000 4.7124
+0.0000 0.0000 4.7124
+0.0000 0.0000 4.7124
+0.0000 0.0000 4.7124
+0.0000 0.0000 4.7124
+primID: 0
+startangle_c: 13
+endpose_c: 1 -2 13
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 5.1051
+0.0056 -0.0111 5.1051
+0.0111 -0.0222 5.1051
+0.0167 -0.0333 5.1051
+0.0222 -0.0444 5.1051
+0.0278 -0.0556 5.1051
+0.0333 -0.0667 5.1051
+0.0389 -0.0778 5.1051
+0.0444 -0.0889 5.1051
+0.0500 -0.1000 5.1051
+primID: 1
+startangle_c: 13
+endpose_c: 3 -6 13
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 5.1051
+0.0167 -0.0333 5.1051
+0.0333 -0.0667 5.1051
+0.0500 -0.1000 5.1051
+0.0667 -0.1333 5.1051
+0.0833 -0.1667 5.1051
+0.1000 -0.2000 5.1051
+0.1167 -0.2333 5.1051
+0.1333 -0.2667 5.1051
+0.1500 -0.3000 5.1051
+primID: 2
+startangle_c: 13
+endpose_c: -1 2 13
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 5.1051
+-0.0056 0.0111 5.1051
+-0.0111 0.0222 5.1051
+-0.0167 0.0333 5.1051
+-0.0222 0.0444 5.1051
+-0.0278 0.0556 5.1051
+-0.0333 0.0667 5.1051
+-0.0389 0.0778 5.1051
+-0.0444 0.0889 5.1051
+-0.0500 0.1000 5.1051
+primID: 3
+startangle_c: 13
+endpose_c: 4 -5 14
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 5.1051
+0.0184 -0.0298 5.1354
+0.0379 -0.0592 5.1657
+0.0583 -0.0881 5.1960
+0.0796 -0.1165 5.2263
+0.1019 -0.1444 5.2566
+0.1251 -0.1717 5.2869
+0.1492 -0.1984 5.3172
+0.1742 -0.2245 5.3475
+0.2000 -0.2500 5.3778
+primID: 4
+startangle_c: 13
+endpose_c: 2 -7 12
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 5.1051
+0.0156 -0.0377 5.1051
+0.0312 -0.0754 5.1051
+0.0468 -0.1131 5.1051
+0.0623 -0.1508 5.0860
+0.0758 -0.1893 5.0113
+0.0863 -0.2287 4.9366
+0.0939 -0.2687 4.8618
+0.0985 -0.3092 4.7871
+0.1000 -0.3500 4.7124
+primID: 5
+startangle_c: 13
+endpose_c: 0 0 14
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 5.1051
+0.0000 0.0000 5.1487
+0.0000 0.0000 5.1924
+0.0000 0.0000 5.2360
+0.0000 0.0000 5.2796
+0.0000 0.0000 5.3233
+0.0000 0.0000 5.3669
+0.0000 0.0000 5.4105
+0.0000 0.0000 5.4542
+0.0000 0.0000 5.4978
+primID: 6
+startangle_c: 13
+endpose_c: 0 0 12
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 5.1051
+0.0000 0.0000 5.0615
+0.0000 0.0000 5.0178
+0.0000 0.0000 4.9742
+0.0000 0.0000 4.9306
+0.0000 0.0000 4.8869
+0.0000 0.0000 4.8433
+0.0000 0.0000 4.7997
+0.0000 0.0000 4.7560
+0.0000 0.0000 4.7124
+primID: 7
+startangle_c: 13
+endpose_c: 0 0 13
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 5.1051
+0.0000 0.0000 5.1051
+0.0000 0.0000 5.1051
+0.0000 0.0000 5.1051
+0.0000 0.0000 5.1051
+0.0000 0.0000 5.1051
+0.0000 0.0000 5.1051
+0.0000 0.0000 5.1051
+0.0000 0.0000 5.1051
+0.0000 0.0000 5.1051
+primID: 8
+startangle_c: 13
+endpose_c: 0 0 13
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 5.1051
+0.0000 0.0000 5.1051
+0.0000 0.0000 5.1051
+0.0000 0.0000 5.1051
+0.0000 0.0000 5.1051
+0.0000 0.0000 5.1051
+0.0000 0.0000 5.1051
+0.0000 0.0000 5.1051
+0.0000 0.0000 5.1051
+0.0000 0.0000 5.1051
+primID: 9
+startangle_c: 13
+endpose_c: 0 0 13
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 5.1051
+0.0000 0.0000 5.1051
+0.0000 0.0000 5.1051
+0.0000 0.0000 5.1051
+0.0000 0.0000 5.1051
+0.0000 0.0000 5.1051
+0.0000 0.0000 5.1051
+0.0000 0.0000 5.1051
+0.0000 0.0000 5.1051
+0.0000 0.0000 5.1051
+primID: 0
+startangle_c: 14
+endpose_c: 1 -1 14
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 5.4978
+0.0056 -0.0056 5.4978
+0.0111 -0.0111 5.4978
+0.0167 -0.0167 5.4978
+0.0222 -0.0222 5.4978
+0.0278 -0.0278 5.4978
+0.0333 -0.0333 5.4978
+0.0389 -0.0389 5.4978
+0.0444 -0.0444 5.4978
+0.0500 -0.0500 5.4978
+primID: 1
+startangle_c: 14
+endpose_c: 6 -6 14
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 5.4978
+0.0333 -0.0333 5.4978
+0.0667 -0.0667 5.4978
+0.1000 -0.1000 5.4978
+0.1333 -0.1333 5.4978
+0.1667 -0.1667 5.4978
+0.2000 -0.2000 5.4978
+0.2333 -0.2333 5.4978
+0.2667 -0.2667 5.4978
+0.3000 -0.3000 5.4978
+primID: 2
+startangle_c: 14
+endpose_c: -1 1 14
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 5.4978
+-0.0056 0.0056 5.4978
+-0.0111 0.0111 5.4978
+-0.0167 0.0167 5.4978
+-0.0222 0.0222 5.4978
+-0.0278 0.0278 5.4978
+-0.0333 0.0333 5.4978
+-0.0389 0.0389 5.4978
+-0.0444 0.0444 5.4978
+-0.0500 0.0500 5.4978
+primID: 3
+startangle_c: 14
+endpose_c: 7 -5 15
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 5.4978
+0.0341 -0.0341 5.4978
+0.0684 -0.0678 5.5275
+0.1043 -0.1000 5.5793
+0.1418 -0.1302 5.6312
+0.1809 -0.1584 5.6830
+0.2213 -0.1846 5.7349
+0.2631 -0.2086 5.7868
+0.3060 -0.2304 5.8386
+0.3500 -0.2500 5.8905
+primID: 4
+startangle_c: 14
+endpose_c: 5 -7 13
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 5.4978
+0.0341 -0.0341 5.4978
+0.0678 -0.0684 5.4681
+0.1000 -0.1043 5.4162
+0.1302 -0.1418 5.3644
+0.1584 -0.1809 5.3125
+0.1846 -0.2213 5.2607
+0.2086 -0.2631 5.2088
+0.2304 -0.3060 5.1569
+0.2500 -0.3500 5.1051
+primID: 5
+startangle_c: 14
+endpose_c: 0 0 15
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 5.4978
+0.0000 0.0000 5.5414
+0.0000 0.0000 5.5851
+0.0000 0.0000 5.6287
+0.0000 0.0000 5.6723
+0.0000 0.0000 5.7160
+0.0000 0.0000 5.7596
+0.0000 0.0000 5.8032
+0.0000 0.0000 5.8469
+0.0000 0.0000 5.8905
+primID: 6
+startangle_c: 14
+endpose_c: 0 0 13
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 5.4978
+0.0000 0.0000 5.4542
+0.0000 0.0000 5.4105
+0.0000 0.0000 5.3669
+0.0000 0.0000 5.3233
+0.0000 0.0000 5.2796
+0.0000 0.0000 5.2360
+0.0000 0.0000 5.1924
+0.0000 0.0000 5.1487
+0.0000 0.0000 5.1051
+primID: 7
+startangle_c: 14
+endpose_c: 0 0 14
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 5.4978
+0.0000 0.0000 5.4978
+0.0000 0.0000 5.4978
+0.0000 0.0000 5.4978
+0.0000 0.0000 5.4978
+0.0000 0.0000 5.4978
+0.0000 0.0000 5.4978
+0.0000 0.0000 5.4978
+0.0000 0.0000 5.4978
+0.0000 0.0000 5.4978
+primID: 8
+startangle_c: 14
+endpose_c: 0 0 14
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 5.4978
+0.0000 0.0000 5.4978
+0.0000 0.0000 5.4978
+0.0000 0.0000 5.4978
+0.0000 0.0000 5.4978
+0.0000 0.0000 5.4978
+0.0000 0.0000 5.4978
+0.0000 0.0000 5.4978
+0.0000 0.0000 5.4978
+0.0000 0.0000 5.4978
+primID: 9
+startangle_c: 14
+endpose_c: 0 0 14
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 5.4978
+0.0000 0.0000 5.4978
+0.0000 0.0000 5.4978
+0.0000 0.0000 5.4978
+0.0000 0.0000 5.4978
+0.0000 0.0000 5.4978
+0.0000 0.0000 5.4978
+0.0000 0.0000 5.4978
+0.0000 0.0000 5.4978
+0.0000 0.0000 5.4978
+primID: 0
+startangle_c: 15
+endpose_c: 2 -1 15
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 5.8905
+0.0111 -0.0056 5.8905
+0.0222 -0.0111 5.8905
+0.0333 -0.0167 5.8905
+0.0444 -0.0222 5.8905
+0.0556 -0.0278 5.8905
+0.0667 -0.0333 5.8905
+0.0778 -0.0389 5.8905
+0.0889 -0.0444 5.8905
+0.1000 -0.0500 5.8905
+primID: 1
+startangle_c: 15
+endpose_c: 6 -3 15
+additionalactioncostmult: 1
+intermediateposes: 10
+0.0000 0.0000 5.8905
+0.0333 -0.0167 5.8905
+0.0667 -0.0333 5.8905
+0.1000 -0.0500 5.8905
+0.1333 -0.0667 5.8905
+0.1667 -0.0833 5.8905
+0.2000 -0.1000 5.8905
+0.2333 -0.1167 5.8905
+0.2667 -0.1333 5.8905
+0.3000 -0.1500 5.8905
+primID: 2
+startangle_c: 15
+endpose_c: -2 1 15
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 5.8905
+-0.0111 0.0056 5.8905
+-0.0222 0.0111 5.8905
+-0.0333 0.0167 5.8905
+-0.0444 0.0222 5.8905
+-0.0556 0.0278 5.8905
+-0.0667 0.0333 5.8905
+-0.0778 0.0389 5.8905
+-0.0889 0.0444 5.8905
+-0.1000 0.0500 5.8905
+primID: 3
+startangle_c: 15
+endpose_c: 5 -4 14
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 5.8905
+0.0298 -0.0184 5.8602
+0.0592 -0.0379 5.8299
+0.0881 -0.0583 5.7996
+0.1165 -0.0796 5.7693
+0.1444 -0.1019 5.7390
+0.1717 -0.1251 5.7087
+0.1984 -0.1492 5.6784
+0.2245 -0.1742 5.6481
+0.2500 -0.2000 5.6178
+primID: 4
+startangle_c: 15
+endpose_c: 7 -2 0
+additionalactioncostmult: 2
+intermediateposes: 10
+0.0000 0.0000 5.8905
+0.0377 -0.0156 5.8905
+0.0754 -0.0312 5.8905
+0.1131 -0.0468 5.8905
+0.1508 -0.0623 5.9096
+0.1893 -0.0758 5.9843
+0.2287 -0.0863 6.0590
+0.2687 -0.0939 6.1337
+0.3092 -0.0985 6.2085
+0.3500 -0.1000 6.2832
+primID: 5
+startangle_c: 15
+endpose_c: 0 0 14
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 5.8905
+0.0000 0.0000 5.8469
+0.0000 0.0000 5.8032
+0.0000 0.0000 5.7596
+0.0000 0.0000 5.7160
+0.0000 0.0000 5.6723
+0.0000 0.0000 5.6287
+0.0000 0.0000 5.5851
+0.0000 0.0000 5.5414
+0.0000 0.0000 5.4978
+primID: 6
+startangle_c: 15
+endpose_c: 0 0 0
+additionalactioncostmult: 5
+intermediateposes: 10
+0.0000 0.0000 5.8905
+0.0000 0.0000 5.9341
+0.0000 0.0000 5.9778
+0.0000 0.0000 6.0214
+0.0000 0.0000 6.0650
+0.0000 0.0000 6.1087
+0.0000 0.0000 6.1523
+0.0000 0.0000 6.1959
+0.0000 0.0000 6.2396
+0.0000 0.0000 0.0000
+primID: 7
+startangle_c: 15
+endpose_c: 0 0 15
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 5.8905
+0.0000 0.0000 5.8905
+0.0000 0.0000 5.8905
+0.0000 0.0000 5.8905
+0.0000 0.0000 5.8905
+0.0000 0.0000 5.8905
+0.0000 0.0000 5.8905
+0.0000 0.0000 5.8905
+0.0000 0.0000 5.8905
+0.0000 0.0000 5.8905
+primID: 8
+startangle_c: 15
+endpose_c: 0 0 15
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 5.8905
+0.0000 0.0000 5.8905
+0.0000 0.0000 5.8905
+0.0000 0.0000 5.8905
+0.0000 0.0000 5.8905
+0.0000 0.0000 5.8905
+0.0000 0.0000 5.8905
+0.0000 0.0000 5.8905
+0.0000 0.0000 5.8905
+0.0000 0.0000 5.8905
+primID: 9
+startangle_c: 15
+endpose_c: 0 0 15
+additionalactioncostmult: 0
+intermediateposes: 10
+0.0000 0.0000 5.8905
+0.0000 0.0000 5.8905
+0.0000 0.0000 5.8905
+0.0000 0.0000 5.8905
+0.0000 0.0000 5.8905
+0.0000 0.0000 5.8905
+0.0000 0.0000 5.8905
+0.0000 0.0000 5.8905
+0.0000 0.0000 5.8905
+0.0000 0.0000 5.8905
diff --git a/navigation/ca_move_base/package.xml b/navigation/ca_move_base/package.xml
index 04a27453..55022967 100644
--- a/navigation/ca_move_base/package.xml
+++ b/navigation/ca_move_base/package.xml
@@ -9,27 +9,27 @@
BSD
catkin
- actionlib
- amcl
- move_base
- move_base_msgs
- roscpp
- tf
- actionlib
- amcl
- move_base
- move_base_msgs
- roscpp
- tf
- actionlib
- amcl
+
+ actionlib
+ amcl
+ map_server
+ move_base
+ move_base_msgs
+ roscpp
+ tf
+
+ mbf_simple_nav
+ smach_ros
+
+ sbpl_lattice_planner
+ global_planner
+
+ eband_local_planner
dwa_local_planner
- move_base
- move_base_msgs
- roscpp
- tf
- map_server
-
-
+ rotate_recovery
+ clear_costmap_recovery
+ moveback_recovery
+
+
diff --git a/navigation/ca_move_base/scripts/mbf_move_base_action.py b/navigation/ca_move_base/scripts/mbf_move_base_action.py
new file mode 100755
index 00000000..99e81a38
--- /dev/null
+++ b/navigation/ca_move_base/scripts/mbf_move_base_action.py
@@ -0,0 +1,93 @@
+#!/usr/bin/env python
+import roslib
+import rospy
+import smach
+import smach_ros
+from mbf_msgs.msg import MoveBaseAction
+from mbf_msgs.msg import MoveBaseResult
+from geometry_msgs.msg import PoseStamped
+from nav_msgs.msg import Path
+from wait_for_goal import WaitForGoal
+
+
+@smach.cb_interface(
+ input_keys=['target_pose', 'controller', 'planner', 'recovery_behaviors'])
+def move_base_goal_cb(userdata, goal):
+ goal.target_pose = userdata.target_pose
+ goal.controller = userdata.controller
+ goal.planner = userdata.planner
+ goal.recovery_behaviors = userdata.recovery_behaviors
+
+
+@smach.cb_interface(
+ outcomes=['success', 'general_failure', 'plan_failure', 'ctrl_failure', 'undefined_failure'],
+ output_keys=['mb_outcome', 'mb_message', 'mb_dist_to_goal', 'mb_angle_to_goal'])
+def move_base_result_cb(userdata, status, result):
+ userdata.mb_outcome = result.outcome
+ userdata.mb_message = result.message
+ userdata.mb_dist_to_goal = result.dist_to_goal
+ userdata.mb_angle_to_goal = result.angle_to_goal
+
+ if result.outcome is MoveBaseResult.SUCCESS:
+ return 'success'
+ elif 10 <= result.outcome < 50:
+ return 'general_failure'
+ elif 50 <= result.outcome < 100:
+ return 'plan_failure'
+ elif 100 <= result.outcome < 150:
+ return 'ctrl_failure'
+ else:
+ return 'undefined_failure'
+
+
+def main():
+ rospy.init_node('mbf_state_machine')
+
+ mbf_sm = smach.StateMachine(outcomes=['aborted', 'succeeded', 'preempted'])
+ mbf_sm.userdata.path = Path()
+ mbf_sm.userdata.previous_state = None
+ mbf_sm.userdata.act_pos = None
+ mbf_sm.userdata.error = None
+ mbf_sm.userdata.error_status = None
+ mbf_sm.userdata.goal_position = None
+ mbf_sm.userdata.recovery_behavior = None
+ mbf_sm.userdata.clear_costmap_flag = False
+ mbf_sm.userdata.controller = 'controller'
+ mbf_sm.userdata.planner = 'planner'
+ mbf_sm.userdata.recovery_behaviors = ['clear_costmap', 'rotate_recovery']
+
+ with mbf_sm:
+ # TODO: The states RECALLED and REJECTED are not implemented in the SimpleActionServer!
+ # TODO: Trying to getState() when no goal is running. You are incorrectly using SimpleActionClient
+ # TODO: smach viewer: http://wiki.ros.org/smach/Tutorials/Smach%20Viewer
+ # TODO: navigation scripts: https://github.com/uos/ceres_robot/tree/master/ceres_navigation/scripts
+ smach.StateMachine.add('WAIT_FOR_GOAL',
+ WaitForGoal(),
+ transitions={'received_goal': 'MOVE_BASE',
+ 'preempted': 'preempted'})
+
+ smach.StateMachine.add('MOVE_BASE',
+ smach_ros.SimpleActionState(
+ 'move_base_flex/move_base',
+ MoveBaseAction,
+ goal_cb=move_base_goal_cb,
+ result_cb=move_base_result_cb
+ ),
+ transitions={
+ 'success': 'WAIT_FOR_GOAL',
+ 'general_failure': 'WAIT_FOR_GOAL',
+ 'plan_failure': 'WAIT_FOR_GOAL',
+ 'ctrl_failure': 'WAIT_FOR_GOAL',
+ 'undefined_failure': 'aborted'}
+ )
+
+ sis = smach_ros.IntrospectionServer('mbf_move_base_server', mbf_sm, '/SM_ROOT')
+ sis.start()
+ outcome = mbf_sm.execute()
+ rospy.spin()
+ sis.stop()
+
+
+if __name__ == "__main__":
+ while not rospy.is_shutdown():
+ main()
diff --git a/navigation/ca_move_base/scripts/wait_for_goal.py b/navigation/ca_move_base/scripts/wait_for_goal.py
new file mode 100755
index 00000000..cb30772b
--- /dev/null
+++ b/navigation/ca_move_base/scripts/wait_for_goal.py
@@ -0,0 +1,41 @@
+import rospy
+import smach
+import threading
+from geometry_msgs.msg import PoseStamped
+
+
+class WaitForGoal(smach.State):
+
+ def __init__(self):
+ smach.State.__init__(self, outcomes=['received_goal', 'preempted'], input_keys=[], output_keys=['target_pose'])
+ self.target_pose = PoseStamped()
+ self.signal = threading.Event()
+ self.subscriber = None
+
+ def execute(self, user_data):
+
+ rospy.loginfo("Waiting for a goal...")
+ self.signal.clear()
+ self.subscriber = rospy.Subscriber('/move_base_simple/goal', PoseStamped, self.goal_callback)
+ while not rospy.is_shutdown() and not self.signal.is_set() and not self.preempt_requested():
+ rospy.logdebug("Waiting for a goal...")
+ self.signal.wait(1)
+
+ if self.preempt_requested() or rospy.is_shutdown():
+ self.service_preempt()
+ return 'preempted'
+
+ user_data.target_pose = self.target_pose
+ pos = self.target_pose.pose.position
+ rospy.loginfo("Received goal pose: (%s, %s, %s)", pos.x, pos.y, pos.z)
+
+ return 'received_goal'
+
+ def goal_callback(self, msg):
+ rospy.logdebug("Received goal pose: %s", str(msg))
+ self.target_pose = msg
+ self.signal.set()
+
+ def request_preempt(self):
+ smach.State.request_preempt(self)
+ self.signal.set()
diff --git a/remote.repos b/remote.repos
index d7516f34..4513ff14 100644
--- a/remote.repos
+++ b/remote.repos
@@ -7,6 +7,10 @@ repositories:
type: git
url: https://github.com/RoboticaUtnFrba/libcreate.git
version: master
+ mbf_tools:
+ type: git
+ url: https://github.com/RoboticaUtnFrba/mbf_tools.git
+ version: master
viso2:
type: git
url: https://github.com/RoboticaUtnFrba/viso2.git