diff --git a/.gitignore b/.gitignore index 6e408bd57..f074fc7fa 100644 --- a/.gitignore +++ b/.gitignore @@ -7,6 +7,9 @@ *.*~ *.swp +# Generated rasa models +/mdr_speech/mdr_rasa/models/*/generated/ + # MongoDB prealloc.* mongod.lock diff --git a/.travis/Dockerfile b/.travis/Dockerfile index fcb2a223f..1583f970a 100644 --- a/.travis/Dockerfile +++ b/.travis/Dockerfile @@ -1,4 +1,4 @@ -FROM bitbots/bitbots-domestic:kinetic +FROM bitbots/bitbots-common:kinetic WORKDIR /travis ADD ./ /travis/src/mas_domestic_robotics diff --git a/README.md b/README.md index 3dd368420..e1ecaf2b5 100644 --- a/README.md +++ b/README.md @@ -2,12 +2,36 @@ # mas_domestic_robotics +## Table of contents + +1. [Summary](#summary) +2. [Dependencies](#dependencies) +3. [Packages](#packages) + 1. [Planning](#planning) + 2. [Naming conventions](#naming-conventions) +4. [Getting started](#getting-started) +5. [Docker images](#docker-images) + 1. [Building the bitbots-domestic image](#building-the-bitbots-domestic-image) + 2. [Travis](#travis) +6. [License](#license) +6. [Acknowledgments](#acknowledgments) + ## Summary This repository contains various core domestic robotics functionalities developed by the Autonomous Systems group at Hochschule Bonn-Rhein-Sieg. The code in this repository is mostly ROS-based and is developed in a robot-independent manner. Robot-dependent code and/or configuration files are hosted in robot-specific repositories, e.g. [mas_cob](https://github.com/b-it-bots/mas_cob) for our Care-O-bot. +# Dependencies + +`mas_domestic_robotics` is not a standalone repository and depends on many other components. A diagram of the major dependencies is shown below: + +![mas_domestic_robotics repository dependency diagram](docs/images/repo_dependency_diagram.png) + +Most of the major dependencies are also developed in-house and, in the interest of modularity, are hosted in separate repositories. + +A more complete set of dependencies can be found in the [`mas-domestic.rosinstall`](mas-domestic.rosinstall) file. + ## Packages The functionalities in this repository are organised based on the main capabilities a domestic robot needs to possess. Each of these has its own ROS metapackage, which further includes a set of packages related to that particular functionality. A rough summary of the contents of these packages contents can be found below: @@ -23,18 +47,17 @@ The functionalities in this repository are organised based on the main capabilit The planning metapackage is where most of the high-level functionalites of our robots are implemented. We are developing our robots as skill-based agents; the planning metapackage includes *action* and *scenario* metapackages because of that. The action metapackage is further divided into metapackages for actions related to particular capabilities, while the scenario metapackage includes domain files and/or state machines for various scenarios (e.g. RoboCup@Home tasks, lab demos, and so forth). In principle, scenarios are built by integrating actions together, which is the main benefit of the skill-based agent framework. -### Naming Conventions +### Naming conventions All packages in our domestic code base start with the `mdr_` suffix; this stands for `mas_domestic_robotics`, which is the name of the top-level directory/ROS metapackage. - ## Getting started b-it-bots members can use [these instructions](https://github.com/b-it-bots/dev-env#setup) to setup a complete development environment for all our robots. For external users, the following instructions should get you a working system: -1. Setup a catkin workspace +1. Set up a catkin workspace ``` mkdir -p ~/catkin_ws/src && cd ~/catkin_ws @@ -46,17 +69,18 @@ For external users, the following instructions should get you a working system: ``` wstool update -t src rosdep install --from-paths src --ignore-src --rosdistro=kinetic -y - ``` - -3. Building your code +3. Build the code ``` catkin build ``` +If you encounter any problems, please check the list of [issues](https://github.com/b-it-bots/mas_domestic_robotics/issues) and open a new one if you don't see a discussion of the problem there. + ## Docker images -To test locally, you need to install [docker](https://docs.docker.com/install/linux/docker-ce/ubuntu/) and [docker-compose](https://docs.docker.com/compose/install/), + +If you want to test locally without installing our software and its dependencies, you need to install [docker](https://docs.docker.com/install/linux/docker-ce/ubuntu/) and [docker-compose](https://docs.docker.com/compose/install/). You can pull the image for this repository: @@ -64,19 +88,17 @@ You can pull the image for this repository: docker pull bitbots/bitbots-domestic:kinetic ``` - ### Building the bitbots-domestic image -To manually build this image, first cd into this repository, then: +To manually build this image, first cd into this repository, then execute ``` docker build -t bitbots/bitbots-domestic:kinetic . - ``` ### Travis -To check locally if your changes will pass the tests: +We use continuous integration to ensure the quality of our software. To check locally if your changes will pass the tests, execute ``` docker-compose build @@ -85,9 +107,10 @@ docker-compose run travis ## License -This project is licensed under the GPLv3 License - see the [LICENSE.md](LICENSE.md) file for details +This project is licensed under the GPLv3 License - see the [LICENSE.md](LICENSE.md) file for details. ## Acknowledgments -* Thanks to the many generations of b-it-bots@Home members. You can see a list of contributors [here](https://github.com/b-it-bots/mas_domestic_robotics/graphs/contributors). -* MAS staff and professors who have provided their advice and support +This software would be impossible without +* the many generations of b-it-bots@Home members. A list of contributors can be found [here](https://github.com/b-it-bots/mas_domestic_robotics/graphs/contributors) +* the MAS staff and professors who have provided their advice and support diff --git a/docker-compose.yml b/docker-compose.yml index 8f5c47660..0e486beaf 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -1,10 +1,8 @@ version: '2' services: travis: - build: - dockerfile: .travis/Dockerfile - context: . image: bitbots/bitbots-domestic:${TRAVIS_COMMIT} + build: . domestic: build: . image: bitbots/bitbots-domestic:kinetic diff --git a/docs/images/repo_dependency_diagram.png b/docs/images/repo_dependency_diagram.png new file mode 100644 index 000000000..6bcfb52c1 Binary files /dev/null and b/docs/images/repo_dependency_diagram.png differ diff --git a/docs/planning_flowchart.html b/docs/planning_flowchart.html new file mode 100644 index 000000000..43dc59774 --- /dev/null +++ b/docs/planning_flowchart.html @@ -0,0 +1,12 @@ + + + +Draw.io Diagram + + + + +
+ + + diff --git a/docs/planning_flowchart.png b/docs/planning_flowchart.png new file mode 100644 index 000000000..197302fc0 Binary files /dev/null and b/docs/planning_flowchart.png differ diff --git a/mas-domestic.rosinstall b/mas-domestic.rosinstall index f687f6097..c42922e0f 100644 --- a/mas-domestic.rosinstall +++ b/mas-domestic.rosinstall @@ -9,13 +9,18 @@ version: kinetic - git: - local-name: mas_navigation - uri: https://github.com/b-it-bots/mas_navigation.git + local-name: mas_navigation_tools + uri: https://github.com/b-it-bots/mas_navigation_tools.git version: kinetic - git: - local-name: mas_perception - uri: https://github.com/b-it-bots/mas_perception.git + local-name: mas_perception_libs + uri: https://github.com/b-it-bots/mas_perception_libs.git + version: kinetic + +- git: + local-name: mas_perception_msgs + uri: https://github.com/b-it-bots/mas_perception_msgs.git version: kinetic - git: @@ -23,6 +28,21 @@ uri: https://github.com/b-it-bots/mas_execution_manager.git version: master +- git: + local-name: mas_knowledge_base + uri: https://github.com/b-it-bots/mas_knowledge_base.git + version: master + +- git: + local-name: action-execution + uri: https://github.com/b-it-bots/action-execution.git + version: master + +- git: + local-name: ftsm + uri: https://github.com/b-it-bots/ftsm.git + version: master + - git: local-name: zbar_ros uri: https://github.com/b-it-bots/zbar_ros.git @@ -38,6 +58,11 @@ uri: https://github.com/KCL-Planning/ROSPlan.git version: master +- git: + local-name: mongodb_store + uri: https://github.com/b-it-bots/mongodb_store.git + version: kinetic-devel + - git: local-name: occupancy_grid_utils uri: https://github.com/b-it-bots/occupancy_grid_utils.git diff --git a/mdr_environments/upload_param.launch b/mdr_environments/upload_param.launch index 5a9f8cd95..59a73bfc4 100644 --- a/mdr_environments/upload_param.launch +++ b/mdr_environments/upload_param.launch @@ -2,10 +2,10 @@ - + - + diff --git a/mdr_hri/mdr_mbot_interface/CMakeLists.txt b/mdr_hri/mdr_mbot_interface/CMakeLists.txt new file mode 100644 index 000000000..5669c95bd --- /dev/null +++ b/mdr_hri/mdr_mbot_interface/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mdr_mbot_interface) + +find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs +) + +catkin_python_setup() + +catkin_package( + CATKIN_DEPENDS +) diff --git a/mdr_hri/mdr_mbot_interface/package.xml b/mdr_hri/mdr_mbot_interface/package.xml new file mode 100644 index 000000000..0028a09fc --- /dev/null +++ b/mdr_hri/mdr_mbot_interface/package.xml @@ -0,0 +1,65 @@ + + + mdr_mbot_interface + 0.0.0 + The mdr_mbot_interface package + + + + + lucy + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + rospy + std_msgs + rospy + std_msgs + rospy + std_msgs + + + + + + + + diff --git a/mdr_hri/mdr_mbot_interface/ros/launch/mbot_interface.launch b/mdr_hri/mdr_mbot_interface/ros/launch/mbot_interface.launch new file mode 100644 index 000000000..2056e1d7a --- /dev/null +++ b/mdr_hri/mdr_mbot_interface/ros/launch/mbot_interface.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/mdr_hri/mdr_mbot_interface/ros/scripts/mbot_interface b/mdr_hri/mdr_mbot_interface/ros/scripts/mbot_interface new file mode 100755 index 000000000..426370b79 --- /dev/null +++ b/mdr_hri/mdr_mbot_interface/ros/scripts/mbot_interface @@ -0,0 +1,9 @@ +#!/usr/bin/env python + +import rospy +from mdr_mbot_interface.mbot_planner import MbotPlanner + +if __name__ == '__main__': + rospy.init_node('mbot_planner', anonymous=False) + planner = MbotPlanner() + planner.wait_for_interpretation() diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/src/mdr_perceive_table/__init__.py b/mdr_hri/mdr_mbot_interface/ros/src/mdr_mbot_interface/__init__.py similarity index 100% rename from mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/src/mdr_perceive_table/__init__.py rename to mdr_hri/mdr_mbot_interface/ros/src/mdr_mbot_interface/__init__.py diff --git a/mdr_hri/mdr_mbot_interface/ros/src/mdr_mbot_interface/mbot_planner.py b/mdr_hri/mdr_mbot_interface/ros/src/mdr_mbot_interface/mbot_planner.py new file mode 100644 index 000000000..2d4075998 --- /dev/null +++ b/mdr_hri/mdr_mbot_interface/ros/src/mdr_mbot_interface/mbot_planner.py @@ -0,0 +1,126 @@ +#!/usr/bin/env python + +import rospy +import copy +import rospkg +from std_msgs.msg import String +import diagnostic_msgs.msg as diag_msgs +import rosplan_dispatch_msgs.msg as plan_dispatch_msgs +from mbot_nlu.msg import Slot, ActionSlot, ActionSlotArray + +class MbotPlanner(object): + ''' + Planner class that publishes the corresponding action depending on the + received interpretation + ''' + def __init__(self): + self.robot_name = rospy.get_param('~robot_name', '') + self.action_completed = False + self.action_failed = False + + self.action_dispatch_pub = rospy.Publisher('/kcl_rosplan/action_dispatch', + plan_dispatch_msgs.ActionDispatch, + queue_size=1) + + # Subscribe to the topic publishing the interpretation of the ocmmand + rospy.Subscriber('~input_interpretation', + ActionSlotArray, + self.interpretationCallback) + rospy.Subscriber('/kcl_rosplan/action_feedback', + plan_dispatch_msgs.ActionFeedback, + self.action_feedback_cb) + + # to publish the recognition + # self.pub_sentence_recog = rospy.Publisher('~output_recognition', ActionSlotArray, queue_size=1) + # flag to indicate that a text was received and needs to be processed + self.interpretation_received = False + # to store the sentece that will be received in the callback + self.received_interpretation = None + # inform the user that the node has initialized + rospy.loginfo("Planner is ready to receive actions") + + def interpretationCallback(self, msg): + self.received_interpretation = msg + self.interpretation_received = True + + def process_interpretation(self): + '''Publishes an action depending on the interpretation received. + ''' + for action in self.received_interpretation.sentence_recognition: + dispatch_msg = self.get_dispatch_msg(action) + if not dispatch_msg.name: + rospy.logwarn('[process_interpretation] Got an unknown action; ignoring request') + continue + + rospy.loginfo('\033[1;36m[process_interpretation] Dispatching action {0}\033[0;37m'.format(dispatch_msg.name)) + self.action_dispatch_pub.publish(dispatch_msg) + while not self.action_completed and not self.action_failed: + rospy.sleep(0.1) + + if self.action_failed: + rospy.logerr('[process_interpretation] {0} could not be executed successfully; aborting plan'.format(action.intention)) + self.action_failed = False + break + + self.action_completed = False + rospy.loginfo('\033[1;36m[process_interpretation] Done dispatching actions\033[0;37m') + + def get_dispatch_msg(self, action): + dispatch_msg = plan_dispatch_msgs.ActionDispatch() + + arg_msg = diag_msgs.KeyValue() + arg_msg.key = 'bot' + arg_msg.value = self.robot_name + dispatch_msg.parameters.append(arg_msg) + + if action.intention == 'go': + dispatch_msg.name = 'move_base' + for slot in action.slots: + arg_msg = diag_msgs.KeyValue() + if slot.type == 'destination': + arg_msg.key = 'to' + arg_msg.value = slot.data + dispatch_msg.parameters.append(arg_msg) + elif action.intention == 'take': + for slot in action.slots: + arg_msg = diag_msgs.KeyValue() + if slot.type == 'object': + arg_msg.key = 'obj' + elif slot.type == 'source': + dispatch_msg.name = 'pick' + arg_msg.key = 'surface' + elif slot.type == 'destination': + dispatch_msg.name = 'place' + arg_msg.key = 'surface' + arg_msg.value = slot.data + dispatch_msg.parameters.append(arg_msg) + return dispatch_msg + + def action_feedback_cb(self, msg): + if msg.status == 'action achieved': + self.action_completed = True + elif msg.status == 'action failed': + self.action_failed = True + + def wait_for_interpretation(self): + while not rospy.is_shutdown(): + if self.interpretation_received == True: + # lower flag + self.interpretation_received = False + + # recognize intention + self.process_interpretation() + # action_slot_array_msg = ActionSlotArray() + # for phrase in recognized_sentence: + # # create empty msg + # action_slot_msg = ActionSlot() + # # fill msg + # action_slot_msg.intention = phrase[0] + # for slot in phrase[1]: + # action_slot_msg.slots.append( Slot(type=slot[0], data=slot[1]) ) + # # append each single action_slot to from the array + # action_slot_array_msg.sentence_recognition.append(copy.deepcopy(action_slot_msg)) + # # publish recognition + # self.pub_sentence_recog.publish(action_slot_array_msg) + # sleep to control the frequency of this node + rospy.sleep(0.1) diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/setup.py b/mdr_hri/mdr_mbot_interface/setup.py similarity index 52% rename from mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/setup.py rename to mdr_hri/mdr_mbot_interface/setup.py index 02478b30f..564dbbdb6 100644 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/setup.py +++ b/mdr_hri/mdr_mbot_interface/setup.py @@ -3,9 +3,10 @@ from distutils.core import setup from catkin_pkg.python_setup import generate_distutils_setup +# for your packages to be recognized by python d = generate_distutils_setup( - packages=['mdr_demo_simple_pick_and_place'], - package_dir={'mdr_demo_simple_pick_and_place': 'ros/src/mdr_demo_simple_pick_and_place'} + packages=['mdr_mbot_interface'], + package_dir={'mdr_mbot_interface': 'ros/src/mdr_mbot_interface'} ) setup(**d) diff --git a/mdr_msgs/mdr_monitoring_msgs/CMakeLists.txt b/mdr_msgs/mdr_monitoring_msgs/CMakeLists.txt index cd5d586bc..3cefff8d2 100644 --- a/mdr_msgs/mdr_monitoring_msgs/CMakeLists.txt +++ b/mdr_msgs/mdr_monitoring_msgs/CMakeLists.txt @@ -13,6 +13,7 @@ add_message_files( AudioEndpoint.msg Endpoints.msg ExecutionState.msg + Event.msg ) generate_messages(DEPENDENCIES std_msgs) diff --git a/mdr_msgs/mdr_monitoring_msgs/msg/Event.msg b/mdr_msgs/mdr_monitoring_msgs/msg/Event.msg new file mode 100644 index 000000000..895091ab9 --- /dev/null +++ b/mdr_msgs/mdr_monitoring_msgs/msg/Event.msg @@ -0,0 +1,2 @@ +time stamp +string description diff --git a/mdr_navigation/mdr_2dnav/package.xml b/mdr_navigation/mdr_2dnav/package.xml index 8a89859cc..868a5ca15 100644 --- a/mdr_navigation/mdr_2dnav/package.xml +++ b/mdr_navigation/mdr_2dnav/package.xml @@ -2,7 +2,7 @@ mdr_2dnav 1.2.0 - 2d base navigation using ros move_base for Care-O-bot 3, Fraunhofer IPA + 2D base navigation using ros move_base MAS robotics @@ -15,8 +15,8 @@ actionlib amcl map_server - mcr_default_env_config - mcr_navigation_tools + mdr_environments + mas_navigation_tools move_base move_base_msgs rospy diff --git a/mdr_navigation/mdr_2dnav/ros/launch/nav_common.launch b/mdr_navigation/mdr_2dnav/ros/launch/nav_common.launch index b2cafc48a..25ee282ec 100644 --- a/mdr_navigation/mdr_2dnav/ros/launch/nav_common.launch +++ b/mdr_navigation/mdr_2dnav/ros/launch/nav_common.launch @@ -5,12 +5,12 @@ - + - - + + diff --git a/mdr_navigation/mdr_2dslam/package.xml b/mdr_navigation/mdr_2dslam/package.xml index cd2ccc4f8..b49f88aae 100644 --- a/mdr_navigation/mdr_2dslam/package.xml +++ b/mdr_navigation/mdr_2dslam/package.xml @@ -2,7 +2,7 @@ mdr_2dslam 1.2.0 - 2d base slam mapping based on ros gmapping for Care-O-bot 3, Fraunhofer IPA + 2D base slam mapping based on ros gmapping MAS robotics diff --git a/mdr_perception/mdr_door_status/CMakeLists.txt b/mdr_perception/mdr_door_status/CMakeLists.txt new file mode 100644 index 000000000..cb5b88fc3 --- /dev/null +++ b/mdr_perception/mdr_door_status/CMakeLists.txt @@ -0,0 +1,40 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mdr_door_status) + +find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs std_msgs rosconsole) + +catkin_package( + CATKIN_DEPENDS roscpp sensor_msgs std_msgs rosconsole +) + +include_directories( + ${catkin_INCLUDE_DIRS} +) + +add_executable(door_status_node ros/src/door_status_node.cpp) + +target_link_libraries(door_status_node + ${catkin_LIBRARIES} +) + +add_executable(door_status_node_mockup ros/src/door_status_node_mockup.cpp) + +target_link_libraries(door_status_node_mockup + ${catkin_LIBRARIES} +) + + +### TESTS +if(CATKIN_ENABLE_TESTING) + find_package(roslaunch REQUIRED) + roslaunch_add_file_check(ros/launch) +endif() + + +### INSTALLS +install( + TARGETS + door_status_node + door_status_node_mockup + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/mdr_perception/mdr_door_status/package.xml b/mdr_perception/mdr_door_status/package.xml new file mode 100644 index 000000000..f9f98b255 --- /dev/null +++ b/mdr_perception/mdr_door_status/package.xml @@ -0,0 +1,29 @@ + + mdr_door_status + 0.1.0 + + door status + + BSD + + Frederik Hegger + + Matthias Füller + Sven Schneider + Frederik Hegger + + catkin + + roscpp + sensor_msgs + std_msgs + rosconsole + + roscpp + sensor_msgs + std_msgs + rosconsole + + roslaunch + + diff --git a/mdr_perception/mdr_door_status/ros/launch/door_status.launch b/mdr_perception/mdr_door_status/ros/launch/door_status.launch new file mode 100644 index 000000000..966e00710 --- /dev/null +++ b/mdr_perception/mdr_door_status/ros/launch/door_status.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/mdr_perception/mdr_door_status/ros/launch/door_status_mockup.launch b/mdr_perception/mdr_door_status/ros/launch/door_status_mockup.launch new file mode 100644 index 000000000..d3dcca057 --- /dev/null +++ b/mdr_perception/mdr_door_status/ros/launch/door_status_mockup.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/mdr_perception/mdr_door_status/ros/src/door_status_node.cpp b/mdr_perception/mdr_door_status/ros/src/door_status_node.cpp new file mode 100644 index 000000000..10b3c1399 --- /dev/null +++ b/mdr_perception/mdr_door_status/ros/src/door_status_node.cpp @@ -0,0 +1,94 @@ +/* + * door_status_node.cpp + * + * Created on: Jan 26, 2011 + * Author: Frederik Hegger + */ + +#include +#include +#include + +sensor_msgs::LaserScanConstPtr g_pLaserScanFront; +bool bIsDoorOpen = false; +double dOpeningAngle = 10.0; +double dDistanceThreshold = 1.0; + +bool publish_door_state() +{ + double dAngle = 0.0; + double dSummedDistance = 0.0; + unsigned int dCountInAngleRange = 0; + + if (g_pLaserScanFront == 0) + return false; + + + //sum up distance in the desired opening angle + dAngle = g_pLaserScanFront->angle_min; + for (unsigned int i = 0; i < g_pLaserScanFront->ranges.size(); ++i, dAngle += g_pLaserScanFront->angle_increment) + { + //check angle against desired range and only take those distance measures + if ((dAngle >= -dOpeningAngle) && (dAngle <= dOpeningAngle)) + { + dSummedDistance += g_pLaserScanFront->ranges[i]; + ++dCountInAngleRange; + } + } + + //calc mean + dSummedDistance /= dCountInAngleRange; + + //check mean distance to decide weather door is open or not + if (dSummedDistance > dDistanceThreshold) + { + ROS_INFO("door status: door is OPEN"); + return true; + } + else + { + ROS_INFO("door status: door is CLOSED"); + return false; + } + + return true; +} + + +void laserScanCallback(const sensor_msgs::LaserScan::ConstPtr& msg_in) +{ + g_pLaserScanFront = msg_in; +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "door_status"); + ros::NodeHandle nh("~"); + + ros::Subscriber subFrontScan = nh.subscribe("scan", 1, laserScanCallback); + ros::Publisher pubDoorStatus = nh.advertise("door_status", 1, true); + + // read params from parameter server file + if (nh.getParam("angular_range", dOpeningAngle) == false) + ROS_WARN("Parameter angular_range not specified in launch file, used default value: %lf degree", dOpeningAngle); + if (nh.getParam("distance_threshold", dDistanceThreshold) == false) + ROS_WARN("Parameter distance_threshold not specified in launch file, used default value: %lf meter", dDistanceThreshold); + + // divide degree by 2 (to have range -angle to angle+) and convert radians + dOpeningAngle = ((dOpeningAngle / 2.0) / 180.0) * M_PI; + + ros::Rate loop_rate(10); + + while (ros::ok()) + { + ros::spinOnce(); + bool open = publish_door_state(); + std_msgs::Bool open_msgs; + open_msgs.data = open; + + pubDoorStatus.publish(open_msgs); + loop_rate.sleep(); + } + + return 0; +} diff --git a/mdr_perception/mdr_door_status/ros/src/door_status_node_mockup.cpp b/mdr_perception/mdr_door_status/ros/src/door_status_node_mockup.cpp new file mode 100644 index 000000000..9eafef72e --- /dev/null +++ b/mdr_perception/mdr_door_status/ros/src/door_status_node_mockup.cpp @@ -0,0 +1,61 @@ +/* + * door_status_node.cpp + * + * Created on: Jan 26, 2011 + * Author: Frederik Hegger + */ + +#include +#include + +bool bIsDoorOpen = false; + +ros::Publisher pubDoorStatus; +ros::Subscriber subChangeDoorStatus; + +std_msgs::Bool bMsg; + +void publishDoorStatus() +{ + bMsg.data = bIsDoorOpen; + pubDoorStatus.publish(bMsg); +} + +void changeDoorStatus(const std_msgs::BoolConstPtr status) +{ + bIsDoorOpen = status->data; + + if (bIsDoorOpen) + { + ROS_INFO("Door status: door is OPEN"); + } + else + { + ROS_INFO("Door status: door is CLOSED"); + } + publishDoorStatus(); +} + + + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "door_status_mockup"); + ros::NodeHandle nh("~"); + + + pubDoorStatus = nh.advertise("door_status", 1, true); + + subChangeDoorStatus = nh.subscribe("change_door_status", 1, changeDoorStatus); + + ros::Rate loop_rate(10); + + publishDoorStatus(); + while (ros::ok()) + { + ros::spinOnce(); + loop_rate.sleep(); + } + + return 0; +} diff --git a/mdr_perception/mdr_object_recognition/CMakeLists.txt b/mdr_perception/mdr_object_recognition/CMakeLists.txt index d5219eff3..92fb1d234 100644 --- a/mdr_perception/mdr_object_recognition/CMakeLists.txt +++ b/mdr_perception/mdr_object_recognition/CMakeLists.txt @@ -6,7 +6,6 @@ find_package(catkin REQUIRED rospy std_msgs sensor_msgs - mcr_object_recognition_mean_circle mas_perception_libs ) diff --git a/mdr_perception/mdr_object_recognition/package.xml b/mdr_perception/mdr_object_recognition/package.xml index 0a5132813..0b8119cc5 100644 --- a/mdr_perception/mdr_object_recognition/package.xml +++ b/mdr_perception/mdr_object_recognition/package.xml @@ -2,10 +2,7 @@ mdr_object_recognition 1.2.0 - This package provides launcher for - mcr_object_recognition_mean_circle. - - + This package contains recognition models for @Home scenarios Shehzad Ahmed MAS robotics @@ -19,5 +16,4 @@ sensor_msgs mas_perception_libs - mcr_object_recognition_mean_circle diff --git a/mdr_perception/mdr_object_recognition/ros/launch/mean_circle.launch b/mdr_perception/mdr_object_recognition/ros/launch/mean_circle.launch deleted file mode 100644 index 76109ba35..000000000 --- a/mdr_perception/mdr_object_recognition/ros/launch/mean_circle.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/mdr_planning/README.md b/mdr_planning/README.md new file mode 100644 index 000000000..f6098c1e6 --- /dev/null +++ b/mdr_planning/README.md @@ -0,0 +1,3 @@ +# `mdr_planning` + +![Planning Diagram](../docs/planning_flowchart.png) diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/scripts/move_arm_action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/scripts/move_arm_action index 125a24dbe..830a2e55f 100755 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/scripts/move_arm_action +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/scripts/move_arm_action @@ -1,65 +1,49 @@ #!/usr/bin/env python import rospy -import smach - -from smach_ros import ActionServerWrapper, IntrospectionServer +import actionlib from mdr_move_arm_action.msg import MoveArmAction -from mdr_move_arm_action.action_states import (SetupMoveArm, MoveArm, - SetActionLibResult) - - -class MoveArmSkill(smach.StateMachine): - def __init__(self, timeout=10): - smach.StateMachine.__init__(self, - outcomes=['OVERALL_SUCCESS', - 'OVERALL_FAILED', 'PREEMPTED'], - input_keys=['move_arm_goal'], - output_keys=['move_arm_feedback', - 'move_arm_result']) - - arm_name = rospy.get_param('~arm_name', 'arm') +from mdr_move_arm_action.action_states import MoveArmSM - with self: - smach.StateMachine.add('SETUP_MOVE_ARM', SetupMoveArm(), - transitions={'succeeded': 'MOVE_ARM', - 'failed': 'SETUP_MOVE_ARM'}) +class MoveArmServer(object): + '''A server exposing an arm moving action. - smach.StateMachine.add('MOVE_ARM', MoveArm(arm_name=arm_name), - transitions={'succeeded': 'SET_ACTION_LIB_SUCCESS', - 'failed': 'SET_ACTION_LIB_FAILED'}) + The server expects the following parameters to be made available on the ROS parameter server: + * arm_name: Name of an arm MoveIt! group - smach.StateMachine.add('SET_ACTION_LIB_FAILED', - SetActionLibResult(False), - transitions={'succeeded': 'OVERALL_FAILED'}) + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de - smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', - SetActionLibResult(True), - transitions={'succeeded': 'OVERALL_SUCCESS'}) + ''' + def __init__(self): + arm_name = rospy.get_param('~arm_name', 'arm') + rospy.loginfo('[move_arm] Initialising state machine') + self.action_sm = MoveArmSM(arm_name=arm_name) + rospy.loginfo('[move_arm] State machine initialised') + + self.action_server = actionlib.SimpleActionServer('move_arm_server', + MoveArmAction, + self.execute, False) + self.action_server.start() + rospy.loginfo('move_arm action server ready') + + def execute(self, goal): + rospy.loginfo('[move_arm] Received an action request') + self.action_sm.goal = goal + self.action_sm.result = None + self.action_sm.execution_requested = True + while not self.action_sm.result: + rospy.sleep(0.05) + self.action_server.set_succeeded(self.action_sm.result) if __name__ == '__main__': rospy.init_node('move_arm_server') - - # construct state machine - sm = MoveArmSkill() - - # smach viewer - sis = IntrospectionServer('move_arm_smach_viewer', sm, - '/MOVE_ARM_SMACH_VIEWER') - sis.start() - - asw = ActionServerWrapper( - server_name='move_arm_server', - action_spec=MoveArmAction, - wrapped_container=sm, - succeeded_outcomes=['OVERALL_SUCCESS'], - aborted_outcomes=['OVERALL_FAILED'], - preempted_outcomes=['PREEMPTED'], - goal_key='move_arm_goal', - feedback_key='move_arm_feedback', - result_key='move_arm_result') - - # Run the server in a background thread - asw.run_server() - rospy.spin() + move_arm_server = MoveArmServer() + try: + move_arm_server.action_sm.run() + while move_arm_server.action_sm.is_running and not rospy.is_shutdown(): + rospy.spin() + except (KeyboardInterrupt, SystemExit): + print('{0} interrupted; exiting...'.format(move_arm_server.action_sm.name)) + move_arm_server.action_sm.stop() diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/action_states.py index e35cfb9e9..ad66ca749 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/action_states.py @@ -2,48 +2,39 @@ import numpy as np import rospy -import smach import moveit_commander +from pyftsm.ftsm import FTSMTransitions +from mas_execution.action_sm_base import ActionSMBase from mdr_move_arm_action.msg import MoveArmGoal, MoveArmFeedback, MoveArmResult from mdr_move_arm_action.dmp import DMPExecutor -class SetupMoveArm(smach.State): - def __init__(self): - smach.State.__init__(self, outcomes=['succeeded', 'failed'], - input_keys=['move_arm_goal'], - output_keys=['move_arm_feedback', 'move_arm_result']) - - def execute(self, userdata): - # consider moving the other components of the robot out of the arm's way, though - # this could be dangerous if the robot is for example close to some furniture item - - feedback = MoveArmFeedback() - feedback.current_state = 'MOVE_ARM' - feedback.message = '[move_arm] moving the arm' - userdata.move_arm_feedback = feedback - - return 'succeeded' - -class MoveArm(smach.State): - def __init__(self, timeout=120.0, arm_name='arm'): - smach.State.__init__(self, input_keys=['move_arm_goal'], - outcomes=['succeeded', 'failed']) +class MoveArmSM(ActionSMBase): + def __init__(self, timeout=120.0, arm_name='arm', max_recovery_attempts=1): + super(MoveArmSM, self).__init__('MoveArm', [], max_recovery_attempts) self.timeout = timeout - self.arm = moveit_commander.MoveGroupCommander(arm_name) + self.arm_name = arm_name + self.arm = None - def execute(self, userdata): + def init(self): + try: + self.arm = moveit_commander.MoveGroupCommander(self.arm_name) + except: + rospy.logerr('[move_arm] %s could not be initialised', self.arm_name) + return FTSMTransitions.INIT_FAILED + return FTSMTransitions.INITIALISED + + def running(self): self.arm.clear_pose_targets() success = False - if userdata.move_arm_goal.goal_type == MoveArmGoal.NAMED_TARGET: - self.arm.set_named_target(userdata.move_arm_goal.named_target) + if self.goal.goal_type == MoveArmGoal.NAMED_TARGET: + self.arm.set_named_target(self.goal.named_target) rospy.loginfo('[move_arm] Planning motion and trying to move arm...') success = self.arm.go(wait=True) - elif userdata.move_arm_goal.goal_type == MoveArmGoal.END_EFFECTOR_POSE: - pose = userdata.move_arm_goal.end_effector_pose - - dmp_name = userdata.move_arm_goal.dmp_name - tau = userdata.move_arm_goal.dmp_tau + elif self.goal.goal_type == MoveArmGoal.END_EFFECTOR_POSE: + pose = self.goal.end_effector_pose + dmp_name = self.goal.dmp_name + tau = self.goal.dmp_tau rospy.loginfo('[move_arm] Planning motion and trying to move arm...') # we use a dynamic motion primitive for moving the arm if one is specified; @@ -56,30 +47,30 @@ def execute(self, userdata): self.arm.set_pose_reference_frame(pose.header.frame_id) self.arm.set_pose_target(pose.pose) success = self.arm.go(wait=True) - elif userdata.move_arm_goal.goal_type == MoveArmGoal.JOINT_VALUES: - joint_values = userdata.move_arm_goal.joint_values + elif self.goal.goal_type == MoveArmGoal.JOINT_VALUES: + joint_values = self.goal.joint_values self.arm.set_joint_value_target(joint_values) rospy.loginfo('[move_arm] Planning motion and trying to move arm...') success = self.arm.go(wait=True) else: rospy.logerr('[move_arm] Invalid target specified; ignoring request') - return 'failed' + self.result = self.set_result(False) + return FTSMTransitions.DONE if not success: rospy.logerr('[move_arm] Arm motion unsuccessful') - return 'failed' + self.result = self.set_result(False) + return FTSMTransitions.DONE + rospy.loginfo('[move_arm] Arm motion successful') - return 'succeeded' + self.result = self.set_result(True) + return FTSMTransitions.DONE -class SetActionLibResult(smach.State): - def __init__(self, result): - smach.State.__init__(self, outcomes=['succeeded'], - input_keys=['move_arm_goal'], - output_keys=['move_arm_feedback', 'move_arm_result']) - self.result = result + def recovering(self): + rospy.sleep(5.) + return FTSMTransitions.DONE_RECOVERING - def execute(self, userdata): + def set_result(self, success): result = MoveArmResult() - result.success = self.result - userdata.move_arm_result = result - return 'succeeded' + result.success = success + return result diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py index 9e4989ad3..ef3c20ba7 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_move_arm_action/ros/src/mdr_move_arm_action/dmp.py @@ -6,6 +6,7 @@ import tf import actionlib from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal +from ros_dmp.srv import GenerateMotion, GenerateMotionRequest from mdr_move_arm_action.roll_dmp import RollDMP @@ -24,13 +25,16 @@ def __init__(self, dmp_name, tau): self.dmp_executor_path_topic = rospy.get_param('~path_topic', '/dmp_executor/path') self.move_base_server = rospy.get_param('~move_base_server', 'move_base/move') + # ros_dmp service + self.motion_client = rospy.ServiceProxy('/generate_motion_service', GenerateMotion) + self.number_of_sampling_points = 30 - self.goal_tolerance = 0.02 + self.goal_tolerance = 0.05 self.vel_publisher_arm = rospy.Publisher(self.cartesian_velocity_topic, TwistStamped, queue_size=1) self.vel_publisher_base = rospy.Publisher(self.base_vel_topic, Twist, queue_size=1) self.feedforward_gain = 30 - self.feedback_gain = 1 + self.feedback_gain = 10 self.sigma_threshold_upper = 0.12 self.sigma_threshold_lower = 0.07 self.base_feedback_gain = 2.0 @@ -55,13 +59,32 @@ def sigma_values_cb(self, msg): def move_base(self): move_base_goal = MoveBaseGoal() move_base_goal.target_pose.header.frame_id = self.map_frame_name - print self.move_base_client.send_goal(move_base_goal) + print(self.move_base_client.send_goal(move_base_goal)) def generate_trajectory(self, goal, initial_pos): - goal = np.array([goal[0], goal[1], goal[2], 0.0, 0.0, 0.0]) - initial_pos = np.array([initial_pos[0], initial_pos[1], initial_pos[2], 0.0, 0.0, 0.0]) - self.roll = RollDMP(self.dmp_name, n_bfs=150) - self.pos, self.vel, self.acc = self.roll.roll(goal, initial_pos, self.tau) + req = GenerateMotionRequest() + req.goal_pose.pose.position.x = goal[0] + req.goal_pose.pose.position.y = goal[1] + req.goal_pose.pose.position.z = goal[2] + req.initial_pose.pose.position.x = initial_pos[0] + req.initial_pose.pose.position.y = initial_pos[1] + req.initial_pose.pose.position.z = initial_pos[2] + req.dmp_name = self.dmp_name + req.tau = self.tau + req.dt = 0.001 + + print('[dmp] Sending trajectory request: ', req) + response = self.motion_client(req) + pos_x = [] + pos_y = [] + pos_z = [] + for state in response.cart_traj.cartesian_state: + pos_x.append(state.pose.position.x) + pos_y.append(state.pose.position.y) + pos_z.append(state.pose.position.z) + self.pos = np.array(pos_x) + self.pos = np.vstack((self.pos, np.array(pos_y))) + self.pos = np.vstack((self.pos, np.array(pos_z))) def tranform_pose(self, pose): #transform goals to odom frame @@ -91,16 +114,17 @@ def publish_path(self): self.path_pub.publish(path) def trajectory_controller(self): - previous_pos = None count = 0 previous_index = 0 - path = self.pos[:, 0:3].T + path = self.pos path_x = path[0, :] path_y = path[1, :] path_z = path[2, :] while not rospy.is_shutdown(): try: - (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame_name, self.palm_link_name, rospy.Time(0)) + (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame_name, + self.palm_link_name, + rospy.Time(0)) break except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue @@ -112,7 +136,9 @@ def trajectory_controller(self): old_pos_index = 0 while distance > self.goal_tolerance and not rospy.is_shutdown(): try: - (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame_name, self.palm_link_name, rospy.Time(0)) + (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame_name, + self.palm_link_name, + rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue current_pos = np.array([trans[0], trans[1], trans[2]]) @@ -226,10 +252,10 @@ def execute(self, goal): self.generate_trajectory(goal, initial_pos) pos = [] - for i in range(self.pos.shape[0]): - pos.append(self.tranform_pose(self.pos[i, 0:3])) + for i in range(self.pos.shape[1]): + pos.append(self.tranform_pose(self.pos[:, i])) pos = np.array(pos) - self.pos = pos + self.pos = pos.T self.publish_path() # transform pose to base link diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/CMakeLists.txt b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/CMakeLists.txt index b8c9cab95..22ed6aa75 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/CMakeLists.txt +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/CMakeLists.txt @@ -16,7 +16,7 @@ find_package(catkin REQUIRED COMPONENTS rosplan_dispatch_msgs rosplan_knowledge_msgs diagnostic_msgs - mcr_perception_msgs + mas_perception_msgs mdr_rosplan_interface mdr_move_arm_action mdr_move_base_action @@ -48,7 +48,7 @@ catkin_package( geometry_msgs trajectory_msgs diagnostic_msgs - mcr_perception_msgs + mas_perception_msgs mdr_rosplan_interface mdr_move_arm_action mdr_move_base_action diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/README.md b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/README.md index 1611dfa81..075f83add 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/README.md +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/README.md @@ -9,6 +9,11 @@ The action includes an action server as well as an action client that interacts ### Goal: * ``geometry_msgs/PoseStamped pose``: An end-effector goal pose +* ``uint32 strategy``: Grasping strategy + +The following constants are also defined in the action goal: +* ``uint32 SIDEWAYS_GRASP=0`` +* ``uint32 TOP_GRASP=1`` ### Result: @@ -73,8 +78,6 @@ The following parameters need to be passed when launching the action client: * ``action_timeout``: Maximum time (in seconds) that we are willing to wait for the action to be executed * ``action_dispatch_topic``: Name of the topic at which the plan dispatcher sends action requests * ``action_feedback_topic``: Name of the topic at which the action sends feedback to the plan dispatcher -* ``knowledge_update_service``: Name of a service used for updating the planning problem as the world changes -* ``attribute_fetching_service``: Name of a service used for retrieving attributes representing the current knowledge about the world * ``grasping_pose_frame``: Name of the frame in which grasping is performed (default: 'base_link') ## Action execution summary @@ -97,7 +100,7 @@ The action performs grasping with respect to the `base_link` frame (even if the * ``rosplan_dispatch_msgs`` * ``rosplan_knowledge_msgs`` * ``diagnostic_msgs`` -* ``mcr_perception_msgs`` +* ``mas_perception_msgs`` * ``mdr_rosplan_interface`` * ``mdr_move_arm_action`` * ``mdr_move_base_action`` diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/config/trajectory_weights/weights_table_grasp.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/config/trajectory_weights/weights_table_grasp.yaml index 6a97b7cbb..2bcb6488b 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/config/trajectory_weights/weights_table_grasp.yaml +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/config/trajectory_weights/weights_table_grasp.yaml @@ -1,207 +1,1667 @@ -pitch: [-35.42493997049422, -37.70819524763274, -41.30898221809194, -46.91255259713179, - -54.67543853474187, -63.46993205757798, -70.71710907291556, -73.40838683036453, - -69.88965835948521, -61.23837968207681, -51.46297199294242, -46.651105552304536, - -53.51199582034473, -77.36775616098751, -119.40733708986616, -173.73781284783232, - -226.1456109740379, -256.9255230366232, -248.19097106023983, -192.5413353804086, - -97.86144890114835, 15.403191745759875, 123.77904323809733, 211.94253960392064, - 278.4370219521786, 331.32494006867165, 376.0838394153919, 404.99584445075004, 397.5504226626487, - 333.15522844035416, 207.40468267623098, 40.23505964106349, -129.5637520862881, -259.90824743283804, - -322.2438715398654, -311.38242615291654, -244.58025251479793, -151.69677985653905, - -62.27097378404166, 4.242548228062668, 41.72701342077948, 54.61937386645844, 52.0678236639199, - 42.46420503183177, 30.605603045097798, 17.831640535863865, 3.9115768409267537, -11.035226656357036, - -25.73694492619038, -38.23525698681982, -46.96747089197185, -51.55412317003241, - -52.72523403265943, -51.52230673716135, -48.47287389295395, -43.418549291861005, - -36.12710203132058, -27.217737390364878, -18.73250127108376, -13.940788921463144, - -16.383140380541224, -28.479814446323395, -50.19840934679473, -78.34588385548898, - -106.9796715308115, -129.0771227243504, -138.96143934459482, -134.39916286752, -117.2699501961774, - -92.4438455558197, -65.54260990887046, -40.863874703900365, -20.503006515797157, - -4.857673288780039, 6.139265967008193, 12.034840538128108, 11.78201008858928, 4.554178806755324, - -9.17603539972846, -27.06452970168944, -45.243054184153266, -59.42498559615973, - -66.20027581755308, -63.910105987553706, -52.76476291531106, -34.26469807990957, - -10.31714588108525, 17.428528508170896, 47.650311361280316, 78.83964513599082, 108.6479511556088, - 133.71712237097847, 150.35674122169067, 155.84935265330853, 149.667219171301, 133.86087612595915, - 112.3745444005185, 89.69640055960969, 69.59508619729118, 54.52489160570828, 45.78344309013908, - 44.04929137036943, 49.782033634194036, 63.15855610291111, 83.57913311970796, 109.088551720991, - 136.1606322569329, 160.14074686489496, 176.30498395284482, 181.14929667909337, 173.36150369292395, - 154.05941286281936, 126.24020279597528, 93.77525021113942, 60.472164188134116, 29.603993012435218, - 3.958702688619639, -13.885207810920182, -21.476471710992165, -16.848997943544642, - 0.7832249798221619, 30.385533864461852, 68.93627511091994, 111.90269031244195, 154.16251910598547, - 190.93981622752082, 218.36838869956537, 233.58094610071046, 234.54607891687763, - 220.01454742143613, 189.79459560980567, 145.2497513417973, 89.62834985791211, 27.797829306827328, - -34.782880289602524, -93.6044248049321, -146.58335798475517, -194.84023705533775, - -242.3963649186648, -294.83526437988365, -357.38490486635334, -433.09803303759264, - -521.7578411720901, -619.8377564762594, -721.4265148901123, -819.6866688138135, - -908.3015435028083, -982.4985157361981, -1039.4942245798738, -1078.4361122467722] -roll: [-94.9809024798238, -89.37326678075686, -74.67496646210842, -49.97877940131072, - -17.593122681262724, 16.06645308336598, 41.71318415962005, 50.60713287019311, 38.51880205462394, - 8.097319900299366, -31.598013501826173, -68.24707622586226, -90.81627135482617, - -93.60576994022517, -77.98514286453168, -51.10227044659737, -22.50696815786439, - -0.5347663027544345, 10.124158386418726, 9.150782393854067, -0.3095242652998163, - -13.68185030980892, -27.163152096763604, -39.030814562375696, -49.5151324263574, - -59.485126018140726, -68.80624552086546, -75.428898637592, -75.92665061062036, -67.34610061221686, - -49.32728657858986, -25.192438385029675, -1.2918864998652975, 15.141124521025375, - 18.667930530166085, 7.7185754208041795, -14.77455802286933, -42.4332819435094, -67.77813685589356, - -84.93421974950635, -91.54423345826856, -89.02137901209117, -81.13057995433573, - -71.8508278902564, -63.789491345075696, -57.835970205818235, -53.8231627143478, - -51.463590908332876, -50.94238773923665, -52.942655659661675, -58.189136134695445, - -66.74155669337485, -77.33961889552769, -87.14688594433058, -92.1973263131889, -88.62131828366893, - -74.32902174902947, -50.44322013852119, -21.67409515681044, 4.817148616819843, 21.806399412879404, - 24.68253720151413, 13.196955457999769, -8.512322146430131, -33.57118762493018, -54.88516872102151, - -67.45252093691383, -69.67085932958665, -63.35440572936342, -52.67299311155918, - -42.53041326451286, -36.97101954194654, -38.07581556440504, -45.58076611309898, - -57.22308967053014, -69.64023776501026, -79.50688077198373, -84.51353160768544, - -83.83257156823157, -77.93238694773984, -67.92280694397907, -54.86274891199053, - -39.45901730010299, -22.31036765250445, -4.4655218706363, 12.1844480635262, 25.094101784610512, - 31.88594109540416, 31.426514019400226, 24.561406129229585, 14.071733616471022, 3.8310309701905063, - -2.4667652078742877, -2.4910925874960017, 4.036658820887705, 15.329450275859665, - 28.159760716196264, 38.92003600168367, 44.79013991534497, 44.63012010630281, 39.28967211649084, - 31.232881080163466, 23.636257019694135, 19.304811972720493, 19.785502677120114, - 24.950320982296397, 33.14353632090505, 41.81125510618662, 48.398727074838696, 51.231113704687615, - 50.09924343700417, 46.35777463863367, 42.492653842074425, 41.28546877990567, 44.839129747983, - 53.785840236969825, 66.95276774389771, 81.62171724086008, 94.3240624864676, 101.92183810445113, - 102.60933075138487, 96.47920077162367, 85.44154179319236, 72.51515576452759, 60.73862809903186, - 52.084774877874715, 46.75751956690583, 43.11719157604091, 38.275825894925006, 29.196437849286824, - 13.971039246031658, -7.127902794056211, -31.207065658218177, -53.168507134733815, - -66.97618274894776, -67.45714153993094, -52.067059591180616, -21.977834522009797, - 17.97026793057221, 60.50618410267125, 97.94258360415643, 124.23605347540033, 136.35854432363232, - 134.57499001187503, 121.7126248618848, 101.89089552853724, 79.27247943521832, 57.21560075628847, - 37.928073989631585, 22.51410879818383] -x: [-23.54082186527676, -23.512905258800767, -23.516224768121276, -23.509127341780307, - -23.454121068234553, -23.37527958550504, -23.398062947937724, -23.714937560177475, - -24.478124554177448, -25.68503685748029, -27.12817117252123, -28.444857414576866, - -29.255164972260452, -29.32533200992744, -28.663538268050935, -27.485379784503554, - -26.068328960660104, -24.58356824095513, -23.00962769946186, -21.200239596466837, - -19.096858163949097, -16.95368487091472, -15.374423042071834, -15.062830081970883, - -16.420337211023146, -19.266187917922952, -22.863442272556544, -26.212763756336138, - -28.43414844400656, -29.064052770088523, -28.17268699560032, -26.28311891920491, - -24.142369984008027, -22.446184009232976, -21.626313157732994, -21.768723364314585, - -22.670561864739494, -23.99022574778685, -25.41383154390955, -26.762121364597242, - -27.994402597101168, -29.11804796936013, -30.06316239908554, -30.607870142419564, - -30.41937426536905, -29.21126868808186, -26.94246526112341, -23.943122787757076, - -20.87495709704674, -18.512790864664698, -17.435764167233458, -17.78028043031174, - -19.180496230067334, -20.915425140594678, -22.174498281441092, -22.3240925499991, - -21.103563664135123, -18.729923248158855, -15.895248710835736, -13.624755694929485, - -12.985496396331671, -14.71180186829232, -18.89175793950522, -24.869265613923826, - -31.43499308439889, -37.24493024420751, -41.28732213685973, -43.18410476572519, - -43.194339294574945, -41.951140068617846, -40.10893022819285, -38.10222626963581, - -36.10877330115395, -34.16247721797567, -32.287238408773284, -30.55543065783834, - -29.06037228818418, -27.855098125159977, -26.91524249105149, -26.147620843657243, - -25.42616007720281, -24.62192925623665, -23.60773081960084, -22.241953853405278, - -20.350308251339122, -17.723112367242447, -14.141853873202399, -9.449513334682687, - -3.6720025070654474, 2.8362755406261857, 9.303036300724449, 14.609576588230889, - 17.5737981092729, 17.371008465565403, 13.903729103835676, 7.920315318993102, 0.7993036840940881, - -5.90970951170443, -10.979658455755517, -13.829074454257093, -14.5755935725983, - -13.85183635092414, -12.500079190061692, -11.274722889530162, -10.641362881668742, - -10.704587515925214, -11.250922772773135, -11.867715076437035, -12.089807887886105, - -11.528372274762951, -9.948640513140584, -7.285309022869411, -3.611258670618167, - 0.9044519621289198, 6.012648194756425, 11.361212296824625, 16.45810776843166, 20.673183290896244, - 23.327284211933843, 23.874543031911173, 22.122886022700243, 18.39546579340212, 13.54233035939333, - 8.770094314293303, 5.339138669234294, 4.239493164175406, 5.965509978348073, 10.462318872054993, - 17.240025852914957, 25.58456988711478, 34.768784271394836, 44.19101560731903, 53.42265111802887, - 62.1967348811316, 70.38947366683362, 78.02811866838204, 85.31956397263224, 92.66238947803912, - 100.60176750352969, 109.7128630951549, 120.43768024118057, 132.93038931028732, 146.9707364427417, - 161.98099303697487, 177.14055334845793, 191.554949364158, 204.4214845608734, 215.14598259878588, - 223.39255572340488, 229.07401275593168] -y: [-48.08181651846675, -49.587117112576244, -50.9623587336631, -52.74638540311553, - -55.339316884123335, -58.187634767212785, -59.19858890679832, -55.18823159102758, - -43.45669279583084, -23.680143811777636, 1.0278742629116866, 24.803094657233476, - 41.56642405836774, 48.12670154484163, 45.66964260010242, 38.41392701146586, 30.559274820896626, - 24.01469229055469, 18.37242623227647, 12.461922135369557, 5.780911723761772, -1.2306825260415035, - -7.855647675106398, -13.850897756070554, -19.628125426393403, -25.766439690878475, - -32.25785903733505, -38.106735397658724, -41.700722135824165, -41.78689726002638, - -38.35450818167012, -32.77088479939355, -27.13457755859672, -23.383886152709735, - -22.698519255950604, -25.30103433852694, -30.459665680447525, -36.587875594965475, - -41.56956109814254, -43.41643845983297, -41.04609408947397, -34.71380717009749, - -25.770834909882954, -15.877657193068476, -6.18267876618329, 3.0042222833118606, - 11.840355394622723, 20.142092255935047, 26.74289840637122, 29.592807279096725, 26.707565820644888, - 17.456235617422454, 3.388730612913831, -11.925225381003171, -23.997617973752774, - -28.962264558977488, -24.922343000496628, -12.598312313006955, 4.970869040522485, - 23.575824375424354, 39.379440684218075, 50.19400929294163, 55.947550262636035, 58.225838746081386, - 59.27299512331673, 61.046406924836795, 64.7113242754852, 70.55113102435496, 78.04412796628496, - 85.97088366315737, 92.64894651093383, 96.42012688093448, 96.28287075303587, 92.33099685071431, - 85.70473506502236, 78.06766687875744, 70.92429970225633, 65.1466098402518, 60.88285588515454, - 57.769140305510135, 55.238497697808114, 52.76635428971193, 50.01647387605132, 46.93679691285544, - 43.83967985214271, 41.42191756324371, 40.63190289145288, 42.344396617716775, 46.9347863760049, - 53.95190906255407, 62.07786213730404, 69.43105232791753, 74.09941024378266, 74.68381768815975, - 70.63554962109797, 62.27282910008037, 50.51258663591012, 36.491359876837045, 21.304370351168693, - 6.013296228474457, -8.122423557377378, -19.438718195250082, -25.98893089734145, - -26.128853719153366, -19.38244812067696, -7.151684031278839, 7.173184693921752, - 18.886623201163076, 23.235809508225923, 16.81931017033493, -1.4029158802374324, - -29.8433104623529, -64.82075623228921, -101.63386062733136, -135.70233578562002, - -163.3136997983213, -181.76783190637622, -189.09357664198424, -183.78138421191431, - -164.93275730603818, -132.8760319352321, -89.87628487198717, -40.3823840377136, - 9.545608199456396, 53.52353674119033, 86.31999893025124, 104.89002936158305, 108.59148361413862, - 98.65485183597463, 77.326293537273, 47.16357178564959, 10.74497388276303, -29.265103349701626, - -69.94908118242546, -108.10390173253013, -140.4074927252729, -163.68216316743766, - -175.12064015744392, -172.44337814636657, -154.07896535435387, -119.47006083653432, - -69.47218086412778, -6.639348762292799, 64.85632773989234, 139.77404187801, 212.67787306545785, - 278.82692966691434, 334.7974511897336, 378.7116450336994, 410.118700189302] -yaw: [-53.959788987781124, -53.30371682466102, -50.30427376032306, -45.009643854796366, - -38.23858486542749, -31.445920900106113, -26.17708607783723, -23.44624165703102, - -23.41933783360354, -25.515961378948077, -28.81688930020335, -32.56774753973497, - -36.529787446838284, -40.964730323552274, -46.18201275969121, -51.81029740608197, - -56.21012654557379, -56.53909770605108, -49.73151173357274, -34.11186523750416, - -10.827794825167219, 15.891304347107724, 40.20849702734764, 57.368805169815346, - 65.95177865876515, 67.97159561584449, 66.64040895830699, 63.589047697786704, 57.82287342197275, - 47.11725574000715, 30.46733711609649, 9.533355760921287, -11.897470382363254, -29.830521402437533, - -41.96018651062839, -48.39143034359017, -50.90765467089368, -51.442211403866196, - -50.88101688407953, -48.972314263916196, -45.17504471685727, -39.55329712338891, - -32.95725845941995, -26.49322988683765, -20.878353476637844, -16.223447732915336, - -12.318730267752962, -9.114208916103852, -7.025318075714705, -6.863715409937331, - -9.418953881919258, -14.892711365810932, -22.46579817424915, -30.24344467810545, - -35.69109978319271, -36.477876243338805, -31.444025538076012, -21.288670881670154, - -8.62375068107963, 2.723377241865635, 8.950421398026423, 7.60130696452477, -1.5512568272241891, - -16.415907780851523, -33.32800892857806, -48.34061652494549, -58.544149663334146, - -62.8778987289918, -62.129401745774736, -58.221245342094605, -53.23124870684945, - -48.65125161003768, -45.139206829597285, -42.67849855974542, -40.883183302808774, - -39.24556037081911, -37.279870735621394, -34.610218273166865, -31.036388602506094, - -26.556600636656302, -21.314340237646586, -15.485545852961351, -9.182367311554216, - -2.4539357174518477, 4.608108661974362, 11.748942419186927, 18.53564938062657, 24.488775175064866, - 29.268641679431983, 32.76438623868329, 35.01433031937393, 36.04103332494177, 35.76699835677284, - 34.10630639596401, 31.16848412968319, 27.407761733176166, 23.587197726368412, 20.558872791588364, - 18.97592238631098, 19.07565937416213, 20.61709103744411, 22.979740291136025, 25.377603643755922, - 27.117925844469166, 27.82778995770433, 27.579205757414403, 26.87117155011749, 26.47346470212829, - 27.18502510687263, 29.588305367087866, 33.87866838785326, 39.818047979613915, 46.81614514744827, - 54.094984627640684, 60.861552883388576, 66.41686398223497, 70.17429953524406, 71.62539144890958, - 70.3348055057656, 66.03071453327549, 58.78227299675599, 49.168265349183585, 38.3031310449789, - 27.633806894374317, 18.533974132396466, 11.83869780529428, 7.516135260286569, 4.634765508402249, - 1.6752451183326715, -2.8928198989370846, -9.947988875516257, -19.190646683273787, - -28.95981792942289, -36.52500660659488, -38.85392526474802, -33.68717119866384, - -20.59129682191029, -1.5875034968128299, 18.968950912034025, 35.341880979175166, - 41.95032447278416, 34.96068739312236, 13.34321915944511, -20.982377451382774, -63.886780871889336, - -110.27067151277551, -155.29388790639618, -195.21850221882582, -227.74597004288862, - -251.94049712978926] -z: [-26.674573624633936, -27.114809002109638, -27.80093370552566, -28.836873185567413, - -30.21881883969757, -31.695951164601357, -32.72354364740953, -32.62183352655716, - -30.90258581567612, -27.583495683677555, -23.297201486579876, -19.087260962922908, - -15.936418055004204, -14.273620356202283, -13.80346926204938, -13.802500150724061, - -13.630619842081579, -13.020757497121616, -11.96881998725885, -10.469674381861992, - -8.452160823356074, -5.952178992153526, -3.225706585502589, -0.5697494663204837, - 1.9808009060413083, 4.68376066797161, 7.7212422921196655, 10.792547098296602, 13.147282962472149, - 14.103314291770715, 13.571455393667573, 12.085886522451826, 10.339296245986802, - 8.68602893983164, 7.033777346134439, 5.120448773462501, 2.8513690211156364, 0.4137469421774722, - -1.8591108055200387, -3.7053995669548203, -5.044430532720517, -5.89457353611374, - -6.160820814559367, -5.4596622601026645, -3.15564680899608, 1.3247372084788658, - 8.009198179393971, 15.980184925805682, 23.347897126275733, 27.749074489647064, 27.248379889872915, - 21.249227180863592, 10.952075671055974, -0.9012226404090096, -10.918446805389463, - -16.22596783788798, -15.453611074433008, -9.153655798428526, 0.49699749838585544, - 10.561454823123338, 18.391516558218946, 22.436392554843273, 22.545534532960506, - 19.747590276327738, 15.689361506049153, 11.99803586193934, 9.79057402569709, 9.448977960828584, - 10.667279692473349, 12.692684350192357, 14.645560060408101, 15.811942817737004, - 15.84203100562251, 14.829843387399771, 13.271968183018519, 11.913283989441497, 11.507542003036887, - 12.558104629802221, 15.134221452069113, 18.84592814794398, 22.998757891465335, 26.871030400509593, - 30.007884926015393, 32.42946843702773, 34.691007713089675, 37.77704102447371, 42.83936321417833, - 50.80550353800271, 61.91981744856823, 75.34762579946161, 89.04033240312027, 100.0468270773533, - 105.30303255039908, 102.67548082168979, 91.82574965851784, 74.46578861532257, 53.83412456102248, - 33.60637700701678, 16.731362600762733, 4.69041935031565, -2.5784558424742117, -6.19851466771825, - -7.652486552004426, -8.203701204838094, -8.60951161090784, -9.098233442799044, -9.48718196283113, - -9.346536633833383, -8.18047015987454, -5.61948916274534, -1.5868054321866703, 3.6274373814820575, - 9.426783721975054, 15.078581176949248, 19.909144614716634, 23.43487153892179, 25.38970214772102, - 25.6741236265473, 24.288580218363613, 21.306889896281145, 16.904641282521364, 11.411558969184892, - 5.333027801777911, -0.7017358032446038, -6.065575734065763, -10.287538553248018, - -13.167739669212384, -14.80357049337103, -15.524777375079806, -15.780272101184652, - -16.035519447260974, -16.72035793136546, -18.231003979213103, -20.961756875344914, - -25.33507117144374, -31.809284959656072, -40.85818085498114, -52.925248528929615, - -68.35651409024713, -87.31513845671434, -109.68639958664852, -134.99539426547764, - -162.3735258302758, -190.60811846102203, -218.28355820802886, -243.98292875048506, - -266.49124025702065, -284.9430654469704, -298.88475826361605, -308.25498187704557] +pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766, + -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698, + -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024, + -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475, + -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925, + -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358, + -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696, + -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558, + -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959, + -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712, + -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775, + -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227, + -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924, + -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524, + -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154, + -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213, + -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968, + -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885, + -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287, + -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954, + -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699, + -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334, + -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104, + -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568, + -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354, + -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645, + -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636, + -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587, + -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638, + -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972, + -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447, + -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077, + -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827, + -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278, + -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243, + -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458, + -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384, + -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971, + -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393, + -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256, + -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992, + -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318, + -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932, + -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875, + -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126, + -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044, + -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763, + -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265, + -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126, + -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113, + -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113, + -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535, + -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696, + -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247, + -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216, + -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353, + -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494, + -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333, + -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105, + -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883, + -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927, + -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884, + -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401, + -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479, + -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717, + -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403, + -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617, + -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946, + -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314, + -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325, + -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804, + -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694, + -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923, + -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567, + -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941, + -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014, + -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199, + -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829, + -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106, + -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166, + -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521, + -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426, + -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046, + -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633, + -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434, + -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539, + -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053, + -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804, + -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645, + -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914, + -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364, + -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178, + -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894, + -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255, + -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792, + -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684, + -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029, + -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007, + -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605, + -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889, + -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275, + -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017, + -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505, + -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556, + -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376, + -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741, + -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233, + -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081, + -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436, + -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614, + -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242, + -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417, + -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184, + -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262, + -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124, + -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636, + -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339, + -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535, + -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313, + -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217, + -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734, + -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575, + -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034, + -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034, + -420.89393277454684] +roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766, + -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698, + -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024, + -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475, + -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925, + -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358, + -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696, + -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558, + -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959, + -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712, + -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775, + -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227, + -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924, + -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524, + -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154, + -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213, + -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968, + -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885, + -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287, + -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954, + -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699, + -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334, + -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104, + -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568, + -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354, + -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645, + -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636, + -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587, + -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638, + -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972, + -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447, + -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077, + -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827, + -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278, + -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243, + -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458, + -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384, + -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971, + -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393, + -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256, + -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992, + -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318, + -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932, + -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875, + -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126, + -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044, + -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763, + -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265, + -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126, + -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113, + -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113, + -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535, + -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696, + -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247, + -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216, + -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353, + -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494, + -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333, + -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105, + -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883, + -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927, + -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884, + -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401, + -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479, + -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717, + -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403, + -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617, + -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946, + -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314, + -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325, + -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804, + -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694, + -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923, + -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567, + -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941, + -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014, + -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199, + -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829, + -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106, + -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166, + -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521, + -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426, + -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046, + -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633, + -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434, + -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539, + -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053, + -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804, + -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645, + -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914, + -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364, + -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178, + -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894, + -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255, + -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792, + -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684, + -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029, + -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007, + -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605, + -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889, + -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275, + -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017, + -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505, + -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556, + -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376, + -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741, + -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233, + -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081, + -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436, + -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614, + -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242, + -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417, + -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184, + -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262, + -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124, + -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636, + -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339, + -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535, + -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313, + -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217, + -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734, + -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575, + -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034, + -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034, + -420.89393277454684] +x: [-131.00261478679988, -121.88886417422353, -111.9050389833139, -102.61368963497223, + -96.05947080078046, -94.12535169815399, -97.8607435820271, -107.14613364957096, + -120.82659085972966, -137.19720078410953, -154.5163549178483, -171.18694825080595, + -185.5568218223094, -195.704691059387, -199.6329868378576, -195.91936207338776, + -184.48915098542318, -167.07793507920195, -147.0574840422674, -128.53128958566316, + -115.04358889413827, -108.56400855374055, -109.19561099927448, -115.57413755530115, + -125.64926844955686, -137.46655539270932, -149.61816709281686, -161.26501144566066, + -171.91636446335687, -181.2216960780747, -188.87007846616464, -194.53289513944736, + -197.79145167358277, -198.0996571089822, -194.8802396222356, -187.78401698176944, + -177.02826515891735, -163.64756841648978, -149.46450567572836, -136.67890898964777, + -127.21395723237792, -122.15440349976664, -121.54367242044958, -124.56838026444774, + -129.9847428341204, -136.57776680869415, -143.4619977851926, -150.1463172881614, + -156.43047062938794, -162.25579819785062, -167.580799896415, -172.2857363783464, + -176.10313080152142, -178.60602094331426, -179.29466139771722, -177.77366590562514, + -173.94835807935246, -168.14763364573156, -161.10576879062413, -153.79437054295704, + -147.18005615509452, -142.03544698559352, -138.8812834694729, -138.0254327793118, + -139.60154077046317, -143.53795283987066, -149.46190564030064, -156.61307764750555, + -163.87292592523232, -169.97827533844975, -173.87402597845175, -175.0512378675456, + -173.7109894894428, -170.68112972530366, -167.11714893622806, -164.10386427943158, + -162.31622495964814, -161.86114761154576, -162.33151047253835, -163.02192881838113, + -163.2095501914243, -162.39248606696069, -160.40951035619491, -157.42206476020834, + -153.79159590063577, -149.91799716532543, -146.1123968434442, -142.55104990302257, + -139.31149673683075, -136.46131741930625, -134.16539879470983, -132.7805579230332, + -132.8976720404706, -135.27842864395015, -140.64645739243105, -149.3508159834204, + -161.00381971456645, -174.2605798886825, -186.9134811438676, -196.38261868432437, + -200.49211090980378, -198.23763830134754, -190.21480773205266, -178.51115567595826, + -166.0801280692791, -155.83164235826004, -149.80028431814193, -148.6986554765198, + -151.96202945833986, -158.18027510739103, -165.68937034330935, -173.07927043758525, + -179.46192687105753, -184.482441275634, -188.1614046703995, -190.6833573748501, + -192.21809683888495, -192.81960540582097, -192.41403431825069, -190.86577715074623, + -188.09301880254696, -184.18878513004086, -179.4955981906823, -174.5892113462465, + -170.1561255168205, -166.79776163996192, -164.8373602238494, -164.2126907946925, + -164.50294621069565, -165.0839384166419, -165.3557930296701, -164.95884643563477, + -163.898412256587, -162.534820649653, -161.44472376807636, -161.20582442689792, + -162.18450627688458, -164.40124437891336, -167.51392693816024, -170.91206169710102, + -173.8749652264376, -175.73083709030058, -175.97063351490164, -174.31004528380663, + -170.72511032331406, -165.48897950595216, -159.20946482285498, -152.82808750163824, + -147.5191667738631, -144.44886967752777, -144.4240273588793, -147.54449659368146, + -153.01617480666846, -159.25176211818498, -164.2877168274527, -166.41140144219656, + -164.78014816559207, -159.78674905727087, -153.00534915065177, -146.70549554069234, + -143.09178186665255, -143.5446658872191, -148.14637102277132, -155.6663204124734, + -164.00416538606794, -170.916565992789, -174.75161613413627, -174.9259359923934, + -171.99663265443107, -167.34336788125873, -162.6147423868518, -159.16048671989986, + -157.6477458947188, -157.96425018036857, -159.39300876091175, -160.95217491299363, + -161.7572333232152, -161.28442609094157, -159.4763125555903, -156.69932273396668, + -153.6090737832136, -150.9888364227075, -149.60413892450302, -150.0809012374839, + -152.79153608790728, -157.7397093479904, -164.46467355593433, -172.0172934256874, + -179.0661158741304, -184.15783677719142, -186.08855140295393, -184.27238379689192, + -178.96525805487465, -171.2372865037682, -162.67650112956372, -154.91277319136154, + -149.12741792008617, -145.72381647659327, -144.27134285886908, -143.72945580899287, + -142.85723879551645, -140.65447875831518, -136.68276519662683, -131.17312406130938, + -124.91209567953047, -118.9751543144055, -114.4168612960534, -112.01955243552163, + -112.15689719095518, -114.77332263747567, -119.44360198084003, -125.471236872446, + -132.00253791160844, -138.1561515203214, -143.17484043177828, -146.58907912429945, + -148.3511438380617, -148.87742279194816, -148.94591893398302, -149.43982973654823, + -150.99302147422435, -153.65215284576814, -156.69307614107936, -158.6972973355033, + -157.91096745292066, -152.8004175957856, -142.62726270976202, -127.83568739093043, + -110.09574251961111, -91.96442744166842, -76.26517339306562, -65.38756088681923, + -60.727455422828086, -62.41769838627875, -69.38103902919622, -79.63028303419432, + -90.6904492501862, -100.0330543381811, -105.46650890222921, -105.47370228165889, + -99.49440285791094, -88.11732926351014, -73.10854974665124, -57.20048636703596, + -43.619919989560685, -35.42935377805215, -34.84789242860901, -42.75227348465364, + -58.50946410168745, -80.17821602052324, -104.99065766337972, -129.94273353227604, + -152.31614487459336, -170.01709776381944, -181.70800379508364, -186.78019457154642, + -185.24347046638363, -177.59665180543396, -164.71393092910603, -147.75511927137686, + -128.09259386421706, -107.2415258381779, -86.77724029789665, -68.22375027449615, + -52.90610923413581, -41.779927174503385, -35.27752870209294, -33.22579478747385, + -34.88146090859914, -39.094144062824924, -44.55933540558859, -50.084953618202576, + -54.78496998250598, -58.13877584276208, -59.90622409641475, -59.945540069741, -58.02266713890794, + -53.710211558038985, -46.44646056518766, -35.76628477961155, -21.64346099537084, + -4.823099943406338, 13.00074336490228, 29.276996809341675, 40.992025758372115, 45.29133769691425, + 40.16014663983825, 24.958013623469462, 0.6430403690297408, -30.37648983818459, -64.70364605521273, + -98.5780766753714, -128.49010205980096, -151.64916889145232, -166.24865624155242, + -171.54102444522545, -167.77666186080413, -156.06265128020192, -138.17902607586348, + -116.36917642692488, -93.10932192371236, -70.85982959536072, -51.803566443571675, + -37.58102962803787, -29.04191085373232, -26.05140387409622, -27.411942444947794, + -30.971298702656785, -33.967139701487234, -33.59780771242022, -27.721946232536823, + -15.50944865087424, 2.165242029053751, 22.75109265450623, 42.51462881068035, 57.39178500711525, + 63.9603626762866, 60.24680348570627, 46.13482203280333, 23.280449691574223, -5.396531697017779, + -36.44546991778746, -66.59572190185905, -93.244393312115, -114.63002544381644, -129.7277803714285, + -137.99626292368302, -139.13919014515983, -133.01230035499455, -119.72497935278233, + -99.88864280213238, -74.88535670756845, -46.99987777401981, -19.290622507018494, + 4.836900082983619, 22.275753850864515, 30.9051736945987, 30.102091149122813, 20.89767984126534, + 5.709426552642763, -12.280539670085691, -29.912602539696113, -44.76409817954616, + -55.587777836517255, -62.37400184312159, -66.06509298635545, -68.05618356119531, + -69.6650415443935, -71.73438308006683, -74.46044693767506, -77.45330286152975, -79.96093287153582, + -81.15272296993173, -80.36351741010003, -77.23600916293604, -71.74756168638527, + -64.14841195780141, -54.85947253693819, -44.377179252428895, -33.21572713174938, + -21.893453397774184, -10.949679527408357, -0.9673128756345823, 7.423219742995925, + 13.573678927669173, 16.882316200266693, 16.880518451590333, 13.321463895553073, + 6.2494544216467425, -3.9662652022608986, -16.64105804227179, -30.827458079441, -45.40427423165941, + -59.19176164715269, -71.07926973763034, -80.1486617898492, -85.77441240031021, -87.6818651318595, + -85.95088814283162, -80.96333944498441, -73.30666637565525, -63.65782924774226, + -52.676828368491805, -40.93506347992221, -28.891766117399765, -16.916080004322772, + -5.3384835550584935, 5.4924058263232025, 15.171162899221802, 23.231051427618734, + 29.178380505050853, 32.559117153019926, 33.042995254450894, 30.506717141912656, + 25.098309221713915, 17.267877434957573, 7.754667766136128, -2.4738852870074073, + -12.328160893560188, -20.73549791063537, -26.79110541102302, -29.892155231726377, + -29.825103487837175, -26.78324113785426, -21.306403690118064, -14.15394397582623, + -6.139545262622449, 2.0335265366227753, 9.89870331065091, 17.286397543623664, 24.307308641748374, + 31.26525601401877, 38.52026696289829, 46.33161129748563, 54.71416332554038, 63.34061174435135, + 71.51679336934421, 78.24704133338132, 82.39018658725793, 82.88576123321361, 79.00811170033668, + 70.59040844420252, 58.15831519340492, 42.928737491281964, 26.660959489767816, 11.387156505595188, + -0.9161323493847441, -8.636955778598946, -10.746100985455664, -6.88302166146115, + 2.6901550272644528, 17.227609362490647, 35.67099777743684, 56.793973316018906, 79.28492204264502, + 101.76799145249652, 122.79490061004348, 140.8566568494178, 154.45830822835748, 162.2730575641024, + 163.35441171879694, 157.35115570417733, 144.6537822686842, 126.41052162787045, 104.38486553091724, + 80.67370587471055, 57.34955312756698, 36.11589971154659, 18.06234868471712, 3.576456180471944, + -7.5771785605298545, -16.047264197592554, -22.625071287068153, -27.99125205987563, + -32.544187050649704, -36.342770320132814, -39.160889188810124, -40.621845381417664, + -40.36566724147911, -38.20206823583871, -34.21329090178294, -28.788359919239177, + -22.587615544563256, -16.449965238654954, -11.263594052219574, -7.82420704968046, + -6.7043106382969215, -8.153633537426817, -12.045315570129013, -17.87558753135798, + -24.817072522162295, -31.818486356751524, -37.73735081273553, -41.48808157884077, + -42.185843095170476, -39.266966337888135, -32.56948414179687, -22.362390213218752, + -9.319317728622229, 5.559271102094557, 21.062973392548795, 35.90860732266084, 48.87612268299202, + 58.92359238196129, 65.26758898268541, 67.42353430763573, 65.20869447959286, 58.716140928984274, + 48.27012084952794, 34.372265068707556, 17.645386644363096, -1.221017147956034, -21.52101235133283, + -42.57613526226664, -63.77089174961588, -84.5781075128901, -104.57331194568057, + -123.43864361815416, -140.9579908724867, -157.005848574077] +y: [-99.19714419940159, -79.16394233266908, -57.87218080909418, -39.28438472983111, + -28.386800564847263, -29.413961436687575, -43.913473953576144, -69.67422837210728, + -101.14133069492664, -131.32148548272554, -154.36897298960562, -167.56000537729983, + -171.8480811942197, -171.05140505400772, -170.15303380273187, -173.34724789149277, + -182.51652550260764, -196.6077612179733, -212.0578803921079, -224.1551729270074, + -228.85174829123696, -224.25993586587964, -211.25206790585622, -193.0143316349947, + -173.74730833399678, -157.0391212367104, -144.63181352345478, -136.04353714588737, + -129.03535351100325, -120.64812612176986, -108.4578159331508, -91.66712058998779, + -71.6845532860305, -51.92454636690193, -36.75664559094005, -29.944229192958293, + -33.26253995101549, -45.8841436370077, -64.72238023555612, -85.55581883977085, -104.43639262659643, + -118.73440682883057, -127.4439469965765, -130.85855117578427, -129.9883150575577, + -126.05068565061471, -120.20491993068747, -113.51752511264922, -107.00751318510588, + -101.6120221067554, -98.037143648059, -96.5901783209796, -97.11117132586354, -99.05464430683851, + -101.69784273360709, -104.39663493912799, -106.78682465771945, -108.86073554241558, + -110.91272059008197, -113.391986702604, -116.71712944928908, -121.10899616745179, + -126.49101079170191, -132.48890828569083, -138.52907052668536, -143.98403266052748, + -148.28386085159676, -150.950353874693, -151.59029328770967, -149.92372750552684, + -145.8893613284024, -139.79752247093995, -132.44040508370935, -125.04855412827735, + -119.03041856644234, -115.55253894189944, -115.13281650745513, -117.42895405278668, + -121.3137432440983, -125.21048574002899, -127.55581585488525, -127.2035779940535, + -123.63075578035654, -116.92546848468061, -107.64003980195744, -96.63277766107122, + -84.99305975953892, -74.04821594432073, -65.34295582835624, -60.46526291514498, + -60.70407650618115, -66.66922658810824, -78.05651925687684, -93.67617143035707, + -111.7360977518272, -130.24058283704147, -147.30871275663227, -161.30727700805863, + -170.86975350241954, -174.96772570963563, -173.1273631811409, -165.72224178554154, + -154.15911230206518, -140.7708520660487, -128.34356994048002, -119.39667357114564, + -115.49538835769575, -116.87807764072566, -122.52767636389038, -130.62314678287214, + -139.1652566178275, -146.52616783065477, -151.74687442934803, -154.55565073083676, + -155.2031643789819, -154.2459690762812, -152.3741747997027, -150.31243759575744, + -148.7646263574894, -148.35253400075652, -149.52454396025652, -152.4546938221956, + -156.97962048361924, -162.61455065772643, -168.65639907954113, -174.33920996159839, + -178.98123335741235, -182.07734590854741, -183.33592461504546, -182.69582244314955, + -180.3576867141039, -176.82992166677565, -172.94908163552213, -169.81480317042735, + -168.6009806288589, -170.26752614676, -175.26636333032138, -183.36316952799345, + -193.6626336687613, -204.8455532302751, -215.5341659113945, -224.642821518713, -231.5841029386231, + -236.27823723741093, -239.00153643062308, -240.1602207969857, -240.0805222435039, + -238.8792739976468, -236.4402664233786, -232.4857796328019, -226.70748164073063, + -218.90832171524943, -209.10849307704876, -197.58443529231656, -184.83728952594203, + -171.51692947117536, -158.34183621150794, -146.03906486693822, -135.2931551378401, + -126.67274641338989, -120.522356840103, -116.8495422307382, -115.26711775119627, + -115.04262249541723, -115.26672709456207, -115.09929773879223, -114.01174175462747, + -111.938154926336, -109.27967994662575, -106.7610444266074, -105.19243755344766, + -105.22191192914899, -107.15903178753426, -110.91294427768851, -116.03909562366141, + -121.85319340040662, -127.56333385695282, -132.3915434702031, -135.68773579313347, + -137.0553530223522, -136.49152106606573, -134.50396087391331, -132.13096362061216, + -130.78914260327244, -131.92152103045797, -136.50685861312238, -144.58292188907654, + -154.9805743147388, -165.42722647501083, -173.05862816904528, -175.2162591822015, + -170.27334476780416, -158.19760009121686, -140.64912083959248, -120.58743118943534, + -101.54354695989151, -86.82037717684916, -78.86963873202836, -78.9730241819731, + -87.208732905478, -102.59534157990022, -123.30579810686955, -146.90854660058451, + -170.66096479222475, -191.89527725965974, -208.48091570828308, -219.25913002134993, + -224.29508133070183, -224.82455615157096, -222.87658961783313, -220.67901822911378, + -220.0419990907876, -221.92371042747453, -226.30932003382122, -232.4142180953472, + -239.1075434889348, -245.3854703853822, -250.72594095429886, -255.21957879524751, + -259.4638629985364, -264.29101495969917, -270.44645552037593, -278.33363978175873, + -287.8997813229622, -298.675957930859, -309.9292117127586, -320.853465714804, -330.7289512679556, + -339.01001631839443, -345.3404207556757, -349.52468900709295, -351.4950882757702, + -351.30843956420114, -349.19099656641225, -345.6266700588403, -341.4559943792539, + -337.9255863077082, -336.6122458269782, -339.1590394133497, -346.81553649131257, + -359.86602049319964, -377.1262582942168, -395.74022625809545, -411.4675647034107, + -419.50864481032374, -415.70559925146244, -397.7689969926942, -366.1070646451192, + -323.9312171058613, -276.5534397525774, -230.0823638333837, -189.94210230887091, + -159.6860391307984, -140.44018425867142, -131.05960164254728, -128.83405351298265, + -130.4331021797765, -132.77378964958245, -133.598664448043, -131.70104308630897, + -126.85857197498154, -119.59605542060795, -110.89414841971599, -101.91886722614183, + -93.80124103359078, -87.46894981676736, -83.52587581326873, -82.18044665496083, + -83.22666982990846, -86.0776105317449, -89.84325002250512, -93.43992023523293, -95.72026823608788, + -95.61789886265437, -92.30211434757715, -85.33026401582156, -74.77072267985237, + -61.2585993757004, -45.950045468071075, -30.363497680157124, -16.130369251753944, + -4.708318964022113, 2.8777155744834513, 6.212983448101315, 5.56140196148832, 1.82269284521927, + -3.6117451657027027, -9.093677781503999, -13.032649828418581, -14.265052388454068, + -12.419628464737789, -8.183075798478319, -3.351394348965721, -0.5798384940055397, + -2.8238203221278537, -12.574618457960362, -31.09374357688851, -57.88946050977006, + -90.62907055552884, -125.54979971296869, -158.26608815757967, -184.7370925829678, + -202.10993695070283, -209.20930494571166, -206.57443001611037, -196.09246662518265, + -180.3847681302668, -162.1383131743949, -143.5430153470272, -125.92848416505213, + -109.62850194687415, -94.06091918539822, -77.99447435013359, -59.96518435506764, + -38.78528395904798, -14.052481549971988, 13.469116037352999, 41.73558027808582, + 67.67668776494482, 87.82105256949613, 99.17127607438123, 100.09686637149439, 90.96891255317551, + 74.31360110106945, 54.401903491602475, 36.37112195454357, 25.122323308838894, 24.299071767105474, + 35.60733796084545, 58.60925251977363, 90.97083953417878, 129.02527666162507, 168.46442192730262, + 204.9931440177265, 234.84578165082002, 255.13458309044964, 264.04869333102295, 260.9405833712635, + 246.33164088648044, 221.85233074958896, 190.1149237954721, 154.50438006044362, 118.87017882929412, + 87.1135432104481, 62.69333693214143, 48.115270426306004, 44.50773230112809, 51.40175250111006, + 66.8050512534824, 87.59010715789734, 110.12403882647259, 130.98843565029367, 147.60355998106695, + 158.5997478392373, 163.85990646713972, 164.25977605801083, 161.2187754414792, 156.2146660131347, + 150.40095323538046, 144.40924932571875, 138.34575838676523, 131.9293358853309, 124.68710400298332, + 116.12724252945577, 105.83914725773546, 93.51279283344225, 78.90515326791086, 61.80006387437529, + 42.00492201327354, 19.40692541387674, -5.917558252890152, -33.573230874271864, -62.74274528444666, + -92.15503567861164, -120.15265932612857, -144.85998708249542, -164.4286388593254, + -177.31873975901365, -182.56747411912127, -179.99849715708353, -170.33317799853793, + -155.174775508723, -136.8502267694815, -118.11425365797368, -101.74838641351045, + -90.11915445215804, -84.78453470268744, -86.24244721296932, -93.89058890376597, + -106.21495538177916, -121.15951154482048, -136.5743658936049, -150.61621971325263, + -161.99385323020581, -170.00781819684602, -174.40729806224658, -175.15102714454724, + -172.18938784282471, -165.3700625780896, -154.51544144123787, -139.64662444269274, + -121.26379807895945, -100.56065686649099, -79.46407820455939, -60.445349586067294, + -46.126337444272174, -38.77528077288329, -39.82686472674805, -49.55634236027296, + -66.99108245478719, -90.07326313903191, -116.01965740549392, -141.78042183347972, + -164.48915325804674, -181.81824580195223, -192.1936050384186, -194.86464874708207, + -189.85637149914302, -177.8444701746055, -159.9936426666511, -137.78899220364173, + -112.8776101254829, -86.92681328173411, -61.4995443419655, -37.946187885812385, + -17.314095829999818, -0.2793256793802397, 12.892666001859144, 22.35087361058214, + 28.623738724825635, 32.547011711050814, 35.16166362154166, 37.59316158505565, 40.9252515429786, + 46.081001294230944, 53.72209705873961, 64.17527284982758, 77.39288792317001, 92.95301794423807, + 110.10246016313017, 127.84307430057432, 145.05732762285365, 160.66258701121782, + 173.77600795348275, 183.86412276724406, 190.84585708339466, 195.11799782738245, + 197.48122046939216, 198.9638867275426, 200.56755651863108, 202.98585640947218, 206.36735753017484, + 210.1940659607301, 213.32513104699035, 214.21331928977128, 211.25089340990704, 203.15829367329167, + 189.30944456733187, 169.9006771128133, 145.91404235801855, 118.88681324803126, 90.55706188617755, + 62.490765591022935, 35.79722544105233, 11.007111877948589, -11.866681217908095, + -33.12202103984155, -53.09189944897362, -71.89093420571022, -89.25892597900295, + -104.53982239765106, -116.79113539836814, -124.98292837106044, -128.22643422109374, + -125.9727544559038, -118.13810580734831, -105.13616058766276, -87.82215430794395, + -67.3716974347737, -45.12687495280552, -22.4432811717548, -0.5658122278379778, 19.44939304727038, + 36.762171246899385, 50.732584441996885, 60.87993008610584, 66.83886074810738, 68.3326809251376, + 65.17495028403165, 57.29773071625812, 44.792950118379224, 27.94724280533802, 7.252695883249127, + -16.615157231293985, -42.849357139418395, -70.58183967568638, -98.95676356556059, + -127.19340520577649, -154.63036780787843, -180.74870760607394] +yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766, + -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698, + -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024, + -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475, + -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925, + -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358, + -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696, + -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558, + -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959, + -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712, + -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775, + -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227, + -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924, + -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524, + -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154, + -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213, + -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968, + -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885, + -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287, + -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954, + -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699, + -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334, + -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104, + -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568, + -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354, + -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645, + -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636, + -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587, + -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638, + -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972, + -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447, + -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077, + -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827, + -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278, + -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243, + -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458, + -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384, + -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971, + -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393, + -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256, + -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992, + -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318, + -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932, + -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875, + -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126, + -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044, + -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763, + -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265, + -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126, + -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113, + -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113, + -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535, + -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696, + -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247, + -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216, + -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353, + -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494, + -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333, + -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105, + -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883, + -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927, + -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884, + -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401, + -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479, + -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717, + -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403, + -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617, + -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946, + -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314, + -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325, + -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804, + -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694, + -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923, + -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567, + -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941, + -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014, + -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199, + -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829, + -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106, + -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166, + -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521, + -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426, + -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046, + -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633, + -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434, + -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539, + -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053, + -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804, + -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645, + -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914, + -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364, + -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178, + -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894, + -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255, + -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792, + -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684, + -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029, + -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007, + -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605, + -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889, + -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275, + -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017, + -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505, + -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556, + -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376, + -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741, + -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233, + -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081, + -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436, + -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614, + -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242, + -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417, + -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184, + -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262, + -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124, + -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636, + -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339, + -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535, + -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313, + -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217, + -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734, + -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575, + -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034, + -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034, + -420.89393277454684] +z: [8.660695913847082, 66.39862553863676, 127.55801351244567, 180.5416567670594, 210.78937741627567, + 205.98510123498664, 161.75492603580668, 84.86410600259634, -7.938204250916348, -95.46180594158663, + -159.6160710741801, -191.12823315769305, -191.1077383247354, -168.75160790760808, + -136.7976917935674, -106.70965336063549, -85.49283736476575, -74.97102632230975, + -72.98493565768426, -75.44155069558714, -78.2857794410305, -78.76734664689184, -75.79970900381551, + -69.60926545532763, -61.02352026219444, -50.731204460839145, -38.784872157834826, + -24.48063415135472, -6.604904189438815, 16.003062940241094, 43.61116166930451, 74.59937262542272, + 104.69471826704422, 127.10591770220823, 134.08234145821157, 119.62663119956167, + 82.25187144352125, 26.508018246556365, -37.60809657045731, -97.50294343727298, -142.05752447785434, + -165.01189926649766, -166.22428468257075, -150.6580066881632, -125.88440608451583, + -99.27427444087671, -76.01596349902346, -58.51176921704588, -46.89688221845846, + -40.0319509439571, -36.428785825300736, -34.84658879993211, -34.526729879516196, + -35.16461507569286, -36.74598621919215, -39.344955846661044, -42.94609674951552, + -47.33366981479339, -52.08037130315903, -56.64311306447958, -60.522268045019175, + -63.40062822000451, -65.19777275447987, -66.03688501834353, -66.16552673973, -65.87798906520248, + -65.46886057404075, -65.21885341822762, -65.38814742633711, -66.18701058321993, + -67.71310218370519, -69.87490985875417, -72.34304758340024, -74.57596554892733, + -75.9445158717209, -75.92859524663281, -74.30883578638903, -71.26712435663396, -67.34312951723155, + -63.24836193520065, -59.60140045704585, -56.690265423268045, -54.35501112675093, + -52.025856935869335, -48.89272594930154, -44.14172332474001, -37.177555708112656, + -27.766047180552132, -16.071976274482385, -2.617381566487062, 11.77309724198873, + 25.976102073264208, 38.53687298107297, 47.718551492321396, 51.76605423223608, 49.38878979505458, + 40.290429255399275, 25.491354807794263, 7.246430327597227, -11.475853168764028, + -27.772349573580907, -39.515015426681174, -45.713572144488516, -46.46431428981413, + -42.61798316112623, -35.37703866753643, -25.9934677516497, -15.628487382325641, + -5.320192334714672, 4.040136476501501, 11.774699698468378, 17.500102100664304, 21.15959975218581, + 22.97185786614963, 23.31019232970664, 22.558042956454567, 20.99209210295209, 18.729017300170803, + 15.74651164213902, 11.966823265504356, 7.376265857048469, 2.1431266936820315, -3.3126251807763514, + -8.343814311527554, -12.216153231932507, -14.333645421496714, -14.465721023384898, + -12.87582842967554, -10.283561071346963, -7.656458262095206, -5.898235147224569, + -5.548464248913495, -6.607585778672925, -8.55034572367405, -10.517780185318616, + -11.61186892570347, -11.181289206074855, -8.998149061838516, -5.278041737321532, + -0.5603284740797135, 4.485651709130345, 9.246488587633161, 13.28299530174339, 16.36756662020893, + 18.466571978647558, 19.704100343285322, 20.32815531831131, 20.677451138832144, 21.13258932476601, + 22.03831545911599, 23.60334269570648, 25.810407707445663, 28.386041772609907, 30.87468716575818, + 32.83047926730481, 34.087238764628125, 35.01230737770861, 36.621360874602765, 40.45070511179779, + 48.15601919282285, 60.917623297793895, 78.84123893360714, 100.59365670804146, 123.46645783546617, + 143.91789347099578, 158.44750364654655, 164.50260889528977, 161.097954064323, 148.96575759387107, + 130.25873364860564, 107.99215885210386, 85.46056101968273, 65.78797889606493, 51.62771188476158, + 44.92502099696882, 46.66619035735277, 56.63547648764704, 73.29795847361908, 93.94368722527568, + 115.14660187881398, 133.44963208761388, 146.06803902842015, 151.38700573272683, + 149.12923984177337, 140.2151559819424, 126.44367536795221, 110.13915686176617, 93.84830850562516, + 80.0820870481205, 71.04848324787652, 68.34789291609472, 72.6765134206983, 83.63977979446459, + 99.77023650435606, 118.77456269522624, 137.94387073801647, 154.60399035259434, 166.49431469790014, + 172.0327438962138, 170.4928109096748, 162.13218328502208, 148.26316136662118, 131.1890050423139, + 113.90461235170372, 99.51589491572973, 90.46166485727544, 87.7622777984477, 90.58353615476729, + 96.33891906881483, 101.3726007035209, 102.04139409749659, 95.84221536672281, 82.19477689643513, + 62.61034213227201, 40.20548872373059, 18.75852748002458, 1.657957295481827, -8.90300742366735, + -12.266306640477236, -9.163238878856461, -1.233167864960895, 9.564495981683649, + 21.43992022175332, 33.051379335207095, 43.56592729978036, 52.568382283902764, 59.9082904825656, + 65.55991544152265, 69.53869459501789, 71.88526118098649, 72.70284621092118, 72.21843649851635, + 70.83368992994872, 69.13827315439758, 67.87334691224184, 67.8505290321987, 69.8442171675916, + 74.47726967536957, 82.11311705302812, 92.75985141769938, 105.99298201503689, 120.91543676261807, + 136.18772657541675, 150.16300634223884, 161.13869607756567, 167.68973106972928, + 168.99857035076562, 165.07365193284676, 156.7707462316869, 145.59585156800276, 133.34673828809346, + 121.70779304048943, 111.92368383650255, 104.63857613545397, 99.9202392953937, 97.42415696314441, + 96.61636301843436, 96.97307879858319, 98.1025495789197, 99.77349073377508, 101.86901694512392, + 104.30470000982287, 106.95197406963027, 109.5967587731362, 111.94423545513219, 113.66135899930364, + 114.4352894158048, 114.02254875294729, 112.27073376540599, 109.10848671943762, 104.5142402049289, + 98.48427593329139, 91.02225206964965, 82.16482996592228, 72.04340823046537, 60.9644383318998, + 49.47596335726721, 38.38208716267078, 28.675265153703773, 21.380090006656577, 17.336295088765645, + 16.98025827773935, 20.1977232072601, 26.30579134436739, 34.1820237918946, 42.50787279716556, + 50.05364436655743, 55.91974464196498, 59.66868428102189, 61.32427965788826, 61.25993059906409, + 60.02892932141973, 58.19725874007958, 56.22498874652909, 54.41596991142811, 52.9291999133868, + 51.82801400185255, 51.138336589289274, 50.892598938155075, 51.146726044509975, 51.96875114225244, + 53.40564661532219, 55.43870808852528, 57.93823519360839, 60.62739916553108, 63.064899619087015, + 64.65655841637202, 64.7055801259163, 62.50676710339701, 57.47927602641731, 49.31661470793777, + 38.11706927966492, 24.4510077185352, 9.330391041091739, -5.928968195692799, -19.927807358089126, + -31.432334046946572, -39.58564206883777, -44.02742340146397, -44.89862410201391, + -42.74448812517229, -38.35747836190584, -32.61145550528262, -26.32954437918894, + -20.20719977067912, -14.789151242435427, -10.48250380540003, -7.582119063177211, + -6.287826913269372, -6.702621306839947, -8.81253570937736, -12.45851173987955, -17.315467703254903, + -22.89273066518226, -28.563759038476075, -33.62421205979971, -37.369384313159216, + -39.17768128465537, -38.587042278968454, -35.35458955341083, -29.49368865852931, + -21.285110785761752, -11.260045417067115, -0.15386876206445768, 11.167509802528333, + 21.80199162922029, 30.914335137198602, 37.83281681446311, 42.1250528920106, 43.6424129336149, + 42.527536031710504, 39.1838083377227, 34.20896823571095, 28.298619788384883, 22.130902226323823, + 16.250738211119412, 10.978234041445447, 6.366580287176825, 2.2267366046830612, -1.780708650958427, + -6.007319377392964, -10.670867462380551, -15.74701218807577, -20.935394624211764, + -25.71475763924228, -29.473174580935297, -31.674804797246836, -32.01284071766054, + -30.503625724620882, -27.49687795069467, -23.603695718045664, -19.567702223835404, + -16.117560259075034, -13.838347771722535, -13.087536620644972, -13.9646827725496, + -16.328965936199715, -19.84970900489466, -24.07290465216959, -28.48977493682292, + -32.59854442331317, -35.95539956520115, -38.2137508958304, -39.15228104770502, -38.692332219912934, + -36.90456601359803, -34.004068329378526, -30.332692841352, -26.327899158206776, + -22.478863588907455, -19.27298194233058, -17.13832959626637, -16.389291255919126, + -17.18279456348231, -19.4913537172405, -23.09696936219348, -27.60749078959273, -32.494672428897246, + -37.15076766150979, -40.95788530079359, -43.36162493187602, -43.93846520775633, + -42.446152023679346, -38.848786666305784, -33.313331043792914, -26.180492183033934, + -17.918310843908614, -9.069298172978261, -0.20074671809499756, 8.136492955152297, + 15.441031108092213, 21.29036758626846, 25.372508549246056, 27.522718958733332, 27.762289494620244, + 26.32911239456066, 23.684885172768034, 20.483910329661786, 17.495388855951955, 15.484035434486861, + 15.069215691030688, 16.59509470023471, 20.047924906971748, 25.048393934050562, 30.927904484071945, + 36.87324018466698, 42.10253497425529, 46.024752154331864, 48.33939153393884, 49.0517871424882, + 48.40580399084933, 46.76081807159332, 44.45507771693884, 41.69825116535199, 38.52289882103032, + 34.803261444171405, 30.327999320714078, 24.898546537842716, 18.420586405729864, + 10.962638364832946, 2.769145218537321, -5.769675941714455, -14.177570162462116, + -21.96287190097139, -28.682775714701705, -33.98993062535793, -37.65744218731808, + -39.584533342726225, -39.78848208739123, -38.38883996706238, -35.58823507210259, + -31.651659030216656, -26.884305680876444, -21.60752145164577, -16.133301088860883, + -10.739449278450719, -5.649094917111724, -1.018762093669203, 3.061876393936713, + 6.558023261640048, 9.468369818928435, 11.800068844557336, 13.547269650304042, 14.679655966520992, + 15.144569931188034, 14.882211925027542, 13.849398415516339, 12.04474239771272, 9.52770863530006, + 6.425839876890219, 2.9278589763920806, -0.735815798162439, -4.320509611419369, -7.596486438809738, + -10.37389619761122, -12.519116524619744, -13.960610192047202, -14.685038419335728, + -14.726561926961596, -14.153639370214792, -13.057964531219815, -11.549353043113381, + -9.758421987878664, -7.846069969532445, -6.015682302494043, -4.521627491850837, + -3.6670759284008367, -3.78622388922754, -5.210529940531722, -8.224269075041521, + -13.019390902549098, -19.661194514880624, -28.07379403356179, -38.04868886403423, + -49.2733259570762, -61.37184021856482, -73.94857243204568, -86.62633925391154, -99.07453382284537, + -111.02550604149512] +pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766, + -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698, + -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024, + -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475, + -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925, + -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358, + -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696, + -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558, + -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959, + -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712, + -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775, + -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227, + -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924, + -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524, + -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154, + -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213, + -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968, + -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885, + -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287, + -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954, + -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699, + -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334, + -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104, + -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568, + -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354, + -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645, + -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636, + -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587, + -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638, + -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972, + -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447, + -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077, + -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827, + -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278, + -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243, + -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458, + -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384, + -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971, + -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393, + -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256, + -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992, + -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318, + -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932, + -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875, + -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126, + -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044, + -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763, + -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265, + -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126, + -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113, + -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113, + -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535, + -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696, + -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247, + -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216, + -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353, + -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494, + -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333, + -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105, + -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883, + -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927, + -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884, + -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401, + -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479, + -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717, + -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403, + -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617, + -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946, + -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314, + -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325, + -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804, + -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694, + -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923, + -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567, + -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941, + -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014, + -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199, + -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829, + -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106, + -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166, + -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521, + -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426, + -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046, + -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633, + -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434, + -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539, + -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053, + -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804, + -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645, + -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914, + -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364, + -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178, + -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894, + -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255, + -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792, + -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684, + -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029, + -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007, + -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605, + -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889, + -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275, + -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017, + -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505, + -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556, + -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376, + -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741, + -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233, + -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081, + -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436, + -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614, + -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242, + -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417, + -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184, + -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262, + -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124, + -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636, + -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339, + -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535, + -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313, + -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217, + -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734, + -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575, + -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034, + -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034, + -420.89393277454684] +roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766, + -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698, + -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024, + -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475, + -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925, + -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358, + -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696, + -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558, + -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959, + -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712, + -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775, + -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227, + -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924, + -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524, + -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154, + -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213, + -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968, + -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885, + -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287, + -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954, + -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699, + -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334, + -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104, + -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568, + -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354, + -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645, + -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636, + -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587, + -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638, + -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972, + -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447, + -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077, + -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827, + -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278, + -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243, + -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458, + -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384, + -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971, + -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393, + -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256, + -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992, + -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318, + -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932, + -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875, + -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126, + -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044, + -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763, + -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265, + -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126, + -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113, + -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113, + -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535, + -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696, + -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247, + -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216, + -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353, + -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494, + -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333, + -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105, + -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883, + -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927, + -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884, + -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401, + -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479, + -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717, + -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403, + -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617, + -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946, + -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314, + -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325, + -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804, + -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694, + -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923, + -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567, + -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941, + -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014, + -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199, + -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829, + -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106, + -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166, + -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521, + -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426, + -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046, + -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633, + -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434, + -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539, + -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053, + -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804, + -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645, + -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914, + -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364, + -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178, + -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894, + -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255, + -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792, + -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684, + -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029, + -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007, + -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605, + -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889, + -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275, + -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017, + -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505, + -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556, + -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376, + -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741, + -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233, + -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081, + -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436, + -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614, + -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242, + -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417, + -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184, + -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262, + -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124, + -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636, + -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339, + -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535, + -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313, + -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217, + -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734, + -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575, + -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034, + -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034, + -420.89393277454684] +x: [-131.00261478679988, -121.88886417422353, -111.9050389833139, -102.61368963497223, + -96.05947080078046, -94.12535169815399, -97.8607435820271, -107.14613364957096, + -120.82659085972966, -137.19720078410953, -154.5163549178483, -171.18694825080595, + -185.5568218223094, -195.704691059387, -199.6329868378576, -195.91936207338776, + -184.48915098542318, -167.07793507920195, -147.0574840422674, -128.53128958566316, + -115.04358889413827, -108.56400855374055, -109.19561099927448, -115.57413755530115, + -125.64926844955686, -137.46655539270932, -149.61816709281686, -161.26501144566066, + -171.91636446335687, -181.2216960780747, -188.87007846616464, -194.53289513944736, + -197.79145167358277, -198.0996571089822, -194.8802396222356, -187.78401698176944, + -177.02826515891735, -163.64756841648978, -149.46450567572836, -136.67890898964777, + -127.21395723237792, -122.15440349976664, -121.54367242044958, -124.56838026444774, + -129.9847428341204, -136.57776680869415, -143.4619977851926, -150.1463172881614, + -156.43047062938794, -162.25579819785062, -167.580799896415, -172.2857363783464, + -176.10313080152142, -178.60602094331426, -179.29466139771722, -177.77366590562514, + -173.94835807935246, -168.14763364573156, -161.10576879062413, -153.79437054295704, + -147.18005615509452, -142.03544698559352, -138.8812834694729, -138.0254327793118, + -139.60154077046317, -143.53795283987066, -149.46190564030064, -156.61307764750555, + -163.87292592523232, -169.97827533844975, -173.87402597845175, -175.0512378675456, + -173.7109894894428, -170.68112972530366, -167.11714893622806, -164.10386427943158, + -162.31622495964814, -161.86114761154576, -162.33151047253835, -163.02192881838113, + -163.2095501914243, -162.39248606696069, -160.40951035619491, -157.42206476020834, + -153.79159590063577, -149.91799716532543, -146.1123968434442, -142.55104990302257, + -139.31149673683075, -136.46131741930625, -134.16539879470983, -132.7805579230332, + -132.8976720404706, -135.27842864395015, -140.64645739243105, -149.3508159834204, + -161.00381971456645, -174.2605798886825, -186.9134811438676, -196.38261868432437, + -200.49211090980378, -198.23763830134754, -190.21480773205266, -178.51115567595826, + -166.0801280692791, -155.83164235826004, -149.80028431814193, -148.6986554765198, + -151.96202945833986, -158.18027510739103, -165.68937034330935, -173.07927043758525, + -179.46192687105753, -184.482441275634, -188.1614046703995, -190.6833573748501, + -192.21809683888495, -192.81960540582097, -192.41403431825069, -190.86577715074623, + -188.09301880254696, -184.18878513004086, -179.4955981906823, -174.5892113462465, + -170.1561255168205, -166.79776163996192, -164.8373602238494, -164.2126907946925, + -164.50294621069565, -165.0839384166419, -165.3557930296701, -164.95884643563477, + -163.898412256587, -162.534820649653, -161.44472376807636, -161.20582442689792, + -162.18450627688458, -164.40124437891336, -167.51392693816024, -170.91206169710102, + -173.8749652264376, -175.73083709030058, -175.97063351490164, -174.31004528380663, + -170.72511032331406, -165.48897950595216, -159.20946482285498, -152.82808750163824, + -147.5191667738631, -144.44886967752777, -144.4240273588793, -147.54449659368146, + -153.01617480666846, -159.25176211818498, -164.2877168274527, -166.41140144219656, + -164.78014816559207, -159.78674905727087, -153.00534915065177, -146.70549554069234, + -143.09178186665255, -143.5446658872191, -148.14637102277132, -155.6663204124734, + -164.00416538606794, -170.916565992789, -174.75161613413627, -174.9259359923934, + -171.99663265443107, -167.34336788125873, -162.6147423868518, -159.16048671989986, + -157.6477458947188, -157.96425018036857, -159.39300876091175, -160.95217491299363, + -161.7572333232152, -161.28442609094157, -159.4763125555903, -156.69932273396668, + -153.6090737832136, -150.9888364227075, -149.60413892450302, -150.0809012374839, + -152.79153608790728, -157.7397093479904, -164.46467355593433, -172.0172934256874, + -179.0661158741304, -184.15783677719142, -186.08855140295393, -184.27238379689192, + -178.96525805487465, -171.2372865037682, -162.67650112956372, -154.91277319136154, + -149.12741792008617, -145.72381647659327, -144.27134285886908, -143.72945580899287, + -142.85723879551645, -140.65447875831518, -136.68276519662683, -131.17312406130938, + -124.91209567953047, -118.9751543144055, -114.4168612960534, -112.01955243552163, + -112.15689719095518, -114.77332263747567, -119.44360198084003, -125.471236872446, + -132.00253791160844, -138.1561515203214, -143.17484043177828, -146.58907912429945, + -148.3511438380617, -148.87742279194816, -148.94591893398302, -149.43982973654823, + -150.99302147422435, -153.65215284576814, -156.69307614107936, -158.6972973355033, + -157.91096745292066, -152.8004175957856, -142.62726270976202, -127.83568739093043, + -110.09574251961111, -91.96442744166842, -76.26517339306562, -65.38756088681923, + -60.727455422828086, -62.41769838627875, -69.38103902919622, -79.63028303419432, + -90.6904492501862, -100.0330543381811, -105.46650890222921, -105.47370228165889, + -99.49440285791094, -88.11732926351014, -73.10854974665124, -57.20048636703596, + -43.619919989560685, -35.42935377805215, -34.84789242860901, -42.75227348465364, + -58.50946410168745, -80.17821602052324, -104.99065766337972, -129.94273353227604, + -152.31614487459336, -170.01709776381944, -181.70800379508364, -186.78019457154642, + -185.24347046638363, -177.59665180543396, -164.71393092910603, -147.75511927137686, + -128.09259386421706, -107.2415258381779, -86.77724029789665, -68.22375027449615, + -52.90610923413581, -41.779927174503385, -35.27752870209294, -33.22579478747385, + -34.88146090859914, -39.094144062824924, -44.55933540558859, -50.084953618202576, + -54.78496998250598, -58.13877584276208, -59.90622409641475, -59.945540069741, -58.02266713890794, + -53.710211558038985, -46.44646056518766, -35.76628477961155, -21.64346099537084, + -4.823099943406338, 13.00074336490228, 29.276996809341675, 40.992025758372115, 45.29133769691425, + 40.16014663983825, 24.958013623469462, 0.6430403690297408, -30.37648983818459, -64.70364605521273, + -98.5780766753714, -128.49010205980096, -151.64916889145232, -166.24865624155242, + -171.54102444522545, -167.77666186080413, -156.06265128020192, -138.17902607586348, + -116.36917642692488, -93.10932192371236, -70.85982959536072, -51.803566443571675, + -37.58102962803787, -29.04191085373232, -26.05140387409622, -27.411942444947794, + -30.971298702656785, -33.967139701487234, -33.59780771242022, -27.721946232536823, + -15.50944865087424, 2.165242029053751, 22.75109265450623, 42.51462881068035, 57.39178500711525, + 63.9603626762866, 60.24680348570627, 46.13482203280333, 23.280449691574223, -5.396531697017779, + -36.44546991778746, -66.59572190185905, -93.244393312115, -114.63002544381644, -129.7277803714285, + -137.99626292368302, -139.13919014515983, -133.01230035499455, -119.72497935278233, + -99.88864280213238, -74.88535670756845, -46.99987777401981, -19.290622507018494, + 4.836900082983619, 22.275753850864515, 30.9051736945987, 30.102091149122813, 20.89767984126534, + 5.709426552642763, -12.280539670085691, -29.912602539696113, -44.76409817954616, + -55.587777836517255, -62.37400184312159, -66.06509298635545, -68.05618356119531, + -69.6650415443935, -71.73438308006683, -74.46044693767506, -77.45330286152975, -79.96093287153582, + -81.15272296993173, -80.36351741010003, -77.23600916293604, -71.74756168638527, + -64.14841195780141, -54.85947253693819, -44.377179252428895, -33.21572713174938, + -21.893453397774184, -10.949679527408357, -0.9673128756345823, 7.423219742995925, + 13.573678927669173, 16.882316200266693, 16.880518451590333, 13.321463895553073, + 6.2494544216467425, -3.9662652022608986, -16.64105804227179, -30.827458079441, -45.40427423165941, + -59.19176164715269, -71.07926973763034, -80.1486617898492, -85.77441240031021, -87.6818651318595, + -85.95088814283162, -80.96333944498441, -73.30666637565525, -63.65782924774226, + -52.676828368491805, -40.93506347992221, -28.891766117399765, -16.916080004322772, + -5.3384835550584935, 5.4924058263232025, 15.171162899221802, 23.231051427618734, + 29.178380505050853, 32.559117153019926, 33.042995254450894, 30.506717141912656, + 25.098309221713915, 17.267877434957573, 7.754667766136128, -2.4738852870074073, + -12.328160893560188, -20.73549791063537, -26.79110541102302, -29.892155231726377, + -29.825103487837175, -26.78324113785426, -21.306403690118064, -14.15394397582623, + -6.139545262622449, 2.0335265366227753, 9.89870331065091, 17.286397543623664, 24.307308641748374, + 31.26525601401877, 38.52026696289829, 46.33161129748563, 54.71416332554038, 63.34061174435135, + 71.51679336934421, 78.24704133338132, 82.39018658725793, 82.88576123321361, 79.00811170033668, + 70.59040844420252, 58.15831519340492, 42.928737491281964, 26.660959489767816, 11.387156505595188, + -0.9161323493847441, -8.636955778598946, -10.746100985455664, -6.88302166146115, + 2.6901550272644528, 17.227609362490647, 35.67099777743684, 56.793973316018906, 79.28492204264502, + 101.76799145249652, 122.79490061004348, 140.8566568494178, 154.45830822835748, 162.2730575641024, + 163.35441171879694, 157.35115570417733, 144.6537822686842, 126.41052162787045, 104.38486553091724, + 80.67370587471055, 57.34955312756698, 36.11589971154659, 18.06234868471712, 3.576456180471944, + -7.5771785605298545, -16.047264197592554, -22.625071287068153, -27.99125205987563, + -32.544187050649704, -36.342770320132814, -39.160889188810124, -40.621845381417664, + -40.36566724147911, -38.20206823583871, -34.21329090178294, -28.788359919239177, + -22.587615544563256, -16.449965238654954, -11.263594052219574, -7.82420704968046, + -6.7043106382969215, -8.153633537426817, -12.045315570129013, -17.87558753135798, + -24.817072522162295, -31.818486356751524, -37.73735081273553, -41.48808157884077, + -42.185843095170476, -39.266966337888135, -32.56948414179687, -22.362390213218752, + -9.319317728622229, 5.559271102094557, 21.062973392548795, 35.90860732266084, 48.87612268299202, + 58.92359238196129, 65.26758898268541, 67.42353430763573, 65.20869447959286, 58.716140928984274, + 48.27012084952794, 34.372265068707556, 17.645386644363096, -1.221017147956034, -21.52101235133283, + -42.57613526226664, -63.77089174961588, -84.5781075128901, -104.57331194568057, + -123.43864361815416, -140.9579908724867, -157.005848574077] +y: [-99.19714419940159, -79.16394233266908, -57.87218080909418, -39.28438472983111, + -28.386800564847263, -29.413961436687575, -43.913473953576144, -69.67422837210728, + -101.14133069492664, -131.32148548272554, -154.36897298960562, -167.56000537729983, + -171.8480811942197, -171.05140505400772, -170.15303380273187, -173.34724789149277, + -182.51652550260764, -196.6077612179733, -212.0578803921079, -224.1551729270074, + -228.85174829123696, -224.25993586587964, -211.25206790585622, -193.0143316349947, + -173.74730833399678, -157.0391212367104, -144.63181352345478, -136.04353714588737, + -129.03535351100325, -120.64812612176986, -108.4578159331508, -91.66712058998779, + -71.6845532860305, -51.92454636690193, -36.75664559094005, -29.944229192958293, + -33.26253995101549, -45.8841436370077, -64.72238023555612, -85.55581883977085, -104.43639262659643, + -118.73440682883057, -127.4439469965765, -130.85855117578427, -129.9883150575577, + -126.05068565061471, -120.20491993068747, -113.51752511264922, -107.00751318510588, + -101.6120221067554, -98.037143648059, -96.5901783209796, -97.11117132586354, -99.05464430683851, + -101.69784273360709, -104.39663493912799, -106.78682465771945, -108.86073554241558, + -110.91272059008197, -113.391986702604, -116.71712944928908, -121.10899616745179, + -126.49101079170191, -132.48890828569083, -138.52907052668536, -143.98403266052748, + -148.28386085159676, -150.950353874693, -151.59029328770967, -149.92372750552684, + -145.8893613284024, -139.79752247093995, -132.44040508370935, -125.04855412827735, + -119.03041856644234, -115.55253894189944, -115.13281650745513, -117.42895405278668, + -121.3137432440983, -125.21048574002899, -127.55581585488525, -127.2035779940535, + -123.63075578035654, -116.92546848468061, -107.64003980195744, -96.63277766107122, + -84.99305975953892, -74.04821594432073, -65.34295582835624, -60.46526291514498, + -60.70407650618115, -66.66922658810824, -78.05651925687684, -93.67617143035707, + -111.7360977518272, -130.24058283704147, -147.30871275663227, -161.30727700805863, + -170.86975350241954, -174.96772570963563, -173.1273631811409, -165.72224178554154, + -154.15911230206518, -140.7708520660487, -128.34356994048002, -119.39667357114564, + -115.49538835769575, -116.87807764072566, -122.52767636389038, -130.62314678287214, + -139.1652566178275, -146.52616783065477, -151.74687442934803, -154.55565073083676, + -155.2031643789819, -154.2459690762812, -152.3741747997027, -150.31243759575744, + -148.7646263574894, -148.35253400075652, -149.52454396025652, -152.4546938221956, + -156.97962048361924, -162.61455065772643, -168.65639907954113, -174.33920996159839, + -178.98123335741235, -182.07734590854741, -183.33592461504546, -182.69582244314955, + -180.3576867141039, -176.82992166677565, -172.94908163552213, -169.81480317042735, + -168.6009806288589, -170.26752614676, -175.26636333032138, -183.36316952799345, + -193.6626336687613, -204.8455532302751, -215.5341659113945, -224.642821518713, -231.5841029386231, + -236.27823723741093, -239.00153643062308, -240.1602207969857, -240.0805222435039, + -238.8792739976468, -236.4402664233786, -232.4857796328019, -226.70748164073063, + -218.90832171524943, -209.10849307704876, -197.58443529231656, -184.83728952594203, + -171.51692947117536, -158.34183621150794, -146.03906486693822, -135.2931551378401, + -126.67274641338989, -120.522356840103, -116.8495422307382, -115.26711775119627, + -115.04262249541723, -115.26672709456207, -115.09929773879223, -114.01174175462747, + -111.938154926336, -109.27967994662575, -106.7610444266074, -105.19243755344766, + -105.22191192914899, -107.15903178753426, -110.91294427768851, -116.03909562366141, + -121.85319340040662, -127.56333385695282, -132.3915434702031, -135.68773579313347, + -137.0553530223522, -136.49152106606573, -134.50396087391331, -132.13096362061216, + -130.78914260327244, -131.92152103045797, -136.50685861312238, -144.58292188907654, + -154.9805743147388, -165.42722647501083, -173.05862816904528, -175.2162591822015, + -170.27334476780416, -158.19760009121686, -140.64912083959248, -120.58743118943534, + -101.54354695989151, -86.82037717684916, -78.86963873202836, -78.9730241819731, + -87.208732905478, -102.59534157990022, -123.30579810686955, -146.90854660058451, + -170.66096479222475, -191.89527725965974, -208.48091570828308, -219.25913002134993, + -224.29508133070183, -224.82455615157096, -222.87658961783313, -220.67901822911378, + -220.0419990907876, -221.92371042747453, -226.30932003382122, -232.4142180953472, + -239.1075434889348, -245.3854703853822, -250.72594095429886, -255.21957879524751, + -259.4638629985364, -264.29101495969917, -270.44645552037593, -278.33363978175873, + -287.8997813229622, -298.675957930859, -309.9292117127586, -320.853465714804, -330.7289512679556, + -339.01001631839443, -345.3404207556757, -349.52468900709295, -351.4950882757702, + -351.30843956420114, -349.19099656641225, -345.6266700588403, -341.4559943792539, + -337.9255863077082, -336.6122458269782, -339.1590394133497, -346.81553649131257, + -359.86602049319964, -377.1262582942168, -395.74022625809545, -411.4675647034107, + -419.50864481032374, -415.70559925146244, -397.7689969926942, -366.1070646451192, + -323.9312171058613, -276.5534397525774, -230.0823638333837, -189.94210230887091, + -159.6860391307984, -140.44018425867142, -131.05960164254728, -128.83405351298265, + -130.4331021797765, -132.77378964958245, -133.598664448043, -131.70104308630897, + -126.85857197498154, -119.59605542060795, -110.89414841971599, -101.91886722614183, + -93.80124103359078, -87.46894981676736, -83.52587581326873, -82.18044665496083, + -83.22666982990846, -86.0776105317449, -89.84325002250512, -93.43992023523293, -95.72026823608788, + -95.61789886265437, -92.30211434757715, -85.33026401582156, -74.77072267985237, + -61.2585993757004, -45.950045468071075, -30.363497680157124, -16.130369251753944, + -4.708318964022113, 2.8777155744834513, 6.212983448101315, 5.56140196148832, 1.82269284521927, + -3.6117451657027027, -9.093677781503999, -13.032649828418581, -14.265052388454068, + -12.419628464737789, -8.183075798478319, -3.351394348965721, -0.5798384940055397, + -2.8238203221278537, -12.574618457960362, -31.09374357688851, -57.88946050977006, + -90.62907055552884, -125.54979971296869, -158.26608815757967, -184.7370925829678, + -202.10993695070283, -209.20930494571166, -206.57443001611037, -196.09246662518265, + -180.3847681302668, -162.1383131743949, -143.5430153470272, -125.92848416505213, + -109.62850194687415, -94.06091918539822, -77.99447435013359, -59.96518435506764, + -38.78528395904798, -14.052481549971988, 13.469116037352999, 41.73558027808582, + 67.67668776494482, 87.82105256949613, 99.17127607438123, 100.09686637149439, 90.96891255317551, + 74.31360110106945, 54.401903491602475, 36.37112195454357, 25.122323308838894, 24.299071767105474, + 35.60733796084545, 58.60925251977363, 90.97083953417878, 129.02527666162507, 168.46442192730262, + 204.9931440177265, 234.84578165082002, 255.13458309044964, 264.04869333102295, 260.9405833712635, + 246.33164088648044, 221.85233074958896, 190.1149237954721, 154.50438006044362, 118.87017882929412, + 87.1135432104481, 62.69333693214143, 48.115270426306004, 44.50773230112809, 51.40175250111006, + 66.8050512534824, 87.59010715789734, 110.12403882647259, 130.98843565029367, 147.60355998106695, + 158.5997478392373, 163.85990646713972, 164.25977605801083, 161.2187754414792, 156.2146660131347, + 150.40095323538046, 144.40924932571875, 138.34575838676523, 131.9293358853309, 124.68710400298332, + 116.12724252945577, 105.83914725773546, 93.51279283344225, 78.90515326791086, 61.80006387437529, + 42.00492201327354, 19.40692541387674, -5.917558252890152, -33.573230874271864, -62.74274528444666, + -92.15503567861164, -120.15265932612857, -144.85998708249542, -164.4286388593254, + -177.31873975901365, -182.56747411912127, -179.99849715708353, -170.33317799853793, + -155.174775508723, -136.8502267694815, -118.11425365797368, -101.74838641351045, + -90.11915445215804, -84.78453470268744, -86.24244721296932, -93.89058890376597, + -106.21495538177916, -121.15951154482048, -136.5743658936049, -150.61621971325263, + -161.99385323020581, -170.00781819684602, -174.40729806224658, -175.15102714454724, + -172.18938784282471, -165.3700625780896, -154.51544144123787, -139.64662444269274, + -121.26379807895945, -100.56065686649099, -79.46407820455939, -60.445349586067294, + -46.126337444272174, -38.77528077288329, -39.82686472674805, -49.55634236027296, + -66.99108245478719, -90.07326313903191, -116.01965740549392, -141.78042183347972, + -164.48915325804674, -181.81824580195223, -192.1936050384186, -194.86464874708207, + -189.85637149914302, -177.8444701746055, -159.9936426666511, -137.78899220364173, + -112.8776101254829, -86.92681328173411, -61.4995443419655, -37.946187885812385, + -17.314095829999818, -0.2793256793802397, 12.892666001859144, 22.35087361058214, + 28.623738724825635, 32.547011711050814, 35.16166362154166, 37.59316158505565, 40.9252515429786, + 46.081001294230944, 53.72209705873961, 64.17527284982758, 77.39288792317001, 92.95301794423807, + 110.10246016313017, 127.84307430057432, 145.05732762285365, 160.66258701121782, + 173.77600795348275, 183.86412276724406, 190.84585708339466, 195.11799782738245, + 197.48122046939216, 198.9638867275426, 200.56755651863108, 202.98585640947218, 206.36735753017484, + 210.1940659607301, 213.32513104699035, 214.21331928977128, 211.25089340990704, 203.15829367329167, + 189.30944456733187, 169.9006771128133, 145.91404235801855, 118.88681324803126, 90.55706188617755, + 62.490765591022935, 35.79722544105233, 11.007111877948589, -11.866681217908095, + -33.12202103984155, -53.09189944897362, -71.89093420571022, -89.25892597900295, + -104.53982239765106, -116.79113539836814, -124.98292837106044, -128.22643422109374, + -125.9727544559038, -118.13810580734831, -105.13616058766276, -87.82215430794395, + -67.3716974347737, -45.12687495280552, -22.4432811717548, -0.5658122278379778, 19.44939304727038, + 36.762171246899385, 50.732584441996885, 60.87993008610584, 66.83886074810738, 68.3326809251376, + 65.17495028403165, 57.29773071625812, 44.792950118379224, 27.94724280533802, 7.252695883249127, + -16.615157231293985, -42.849357139418395, -70.58183967568638, -98.95676356556059, + -127.19340520577649, -154.63036780787843, -180.74870760607394] +yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766, + -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698, + -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024, + -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475, + -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925, + -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358, + -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696, + -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558, + -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959, + -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712, + -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775, + -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227, + -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924, + -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524, + -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154, + -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213, + -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968, + -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885, + -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287, + -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954, + -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699, + -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334, + -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104, + -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568, + -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354, + -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645, + -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636, + -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587, + -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638, + -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972, + -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447, + -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077, + -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827, + -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278, + -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243, + -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458, + -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384, + -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971, + -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393, + -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256, + -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992, + -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318, + -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932, + -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875, + -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126, + -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044, + -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763, + -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265, + -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126, + -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113, + -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113, + -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535, + -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696, + -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247, + -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216, + -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353, + -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494, + -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333, + -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105, + -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883, + -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927, + -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884, + -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401, + -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479, + -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717, + -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403, + -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617, + -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946, + -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314, + -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325, + -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804, + -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694, + -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923, + -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567, + -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941, + -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014, + -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199, + -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829, + -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106, + -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166, + -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521, + -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426, + -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046, + -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633, + -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434, + -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539, + -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053, + -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804, + -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645, + -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914, + -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364, + -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178, + -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894, + -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255, + -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792, + -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684, + -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029, + -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007, + -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605, + -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889, + -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275, + -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017, + -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505, + -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556, + -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376, + -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741, + -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233, + -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081, + -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436, + -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614, + -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242, + -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417, + -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184, + -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262, + -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124, + -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636, + -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339, + -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535, + -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313, + -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217, + -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734, + -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575, + -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034, + -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034, + -420.89393277454684] +z: [8.660695913847082, 66.39862553863676, 127.55801351244567, 180.5416567670594, 210.78937741627567, + 205.98510123498664, 161.75492603580668, 84.86410600259634, -7.938204250916348, -95.46180594158663, + -159.6160710741801, -191.12823315769305, -191.1077383247354, -168.75160790760808, + -136.7976917935674, -106.70965336063549, -85.49283736476575, -74.97102632230975, + -72.98493565768426, -75.44155069558714, -78.2857794410305, -78.76734664689184, -75.79970900381551, + -69.60926545532763, -61.02352026219444, -50.731204460839145, -38.784872157834826, + -24.48063415135472, -6.604904189438815, 16.003062940241094, 43.61116166930451, 74.59937262542272, + 104.69471826704422, 127.10591770220823, 134.08234145821157, 119.62663119956167, + 82.25187144352125, 26.508018246556365, -37.60809657045731, -97.50294343727298, -142.05752447785434, + -165.01189926649766, -166.22428468257075, -150.6580066881632, -125.88440608451583, + -99.27427444087671, -76.01596349902346, -58.51176921704588, -46.89688221845846, + -40.0319509439571, -36.428785825300736, -34.84658879993211, -34.526729879516196, + -35.16461507569286, -36.74598621919215, -39.344955846661044, -42.94609674951552, + -47.33366981479339, -52.08037130315903, -56.64311306447958, -60.522268045019175, + -63.40062822000451, -65.19777275447987, -66.03688501834353, -66.16552673973, -65.87798906520248, + -65.46886057404075, -65.21885341822762, -65.38814742633711, -66.18701058321993, + -67.71310218370519, -69.87490985875417, -72.34304758340024, -74.57596554892733, + -75.9445158717209, -75.92859524663281, -74.30883578638903, -71.26712435663396, -67.34312951723155, + -63.24836193520065, -59.60140045704585, -56.690265423268045, -54.35501112675093, + -52.025856935869335, -48.89272594930154, -44.14172332474001, -37.177555708112656, + -27.766047180552132, -16.071976274482385, -2.617381566487062, 11.77309724198873, + 25.976102073264208, 38.53687298107297, 47.718551492321396, 51.76605423223608, 49.38878979505458, + 40.290429255399275, 25.491354807794263, 7.246430327597227, -11.475853168764028, + -27.772349573580907, -39.515015426681174, -45.713572144488516, -46.46431428981413, + -42.61798316112623, -35.37703866753643, -25.9934677516497, -15.628487382325641, + -5.320192334714672, 4.040136476501501, 11.774699698468378, 17.500102100664304, 21.15959975218581, + 22.97185786614963, 23.31019232970664, 22.558042956454567, 20.99209210295209, 18.729017300170803, + 15.74651164213902, 11.966823265504356, 7.376265857048469, 2.1431266936820315, -3.3126251807763514, + -8.343814311527554, -12.216153231932507, -14.333645421496714, -14.465721023384898, + -12.87582842967554, -10.283561071346963, -7.656458262095206, -5.898235147224569, + -5.548464248913495, -6.607585778672925, -8.55034572367405, -10.517780185318616, + -11.61186892570347, -11.181289206074855, -8.998149061838516, -5.278041737321532, + -0.5603284740797135, 4.485651709130345, 9.246488587633161, 13.28299530174339, 16.36756662020893, + 18.466571978647558, 19.704100343285322, 20.32815531831131, 20.677451138832144, 21.13258932476601, + 22.03831545911599, 23.60334269570648, 25.810407707445663, 28.386041772609907, 30.87468716575818, + 32.83047926730481, 34.087238764628125, 35.01230737770861, 36.621360874602765, 40.45070511179779, + 48.15601919282285, 60.917623297793895, 78.84123893360714, 100.59365670804146, 123.46645783546617, + 143.91789347099578, 158.44750364654655, 164.50260889528977, 161.097954064323, 148.96575759387107, + 130.25873364860564, 107.99215885210386, 85.46056101968273, 65.78797889606493, 51.62771188476158, + 44.92502099696882, 46.66619035735277, 56.63547648764704, 73.29795847361908, 93.94368722527568, + 115.14660187881398, 133.44963208761388, 146.06803902842015, 151.38700573272683, + 149.12923984177337, 140.2151559819424, 126.44367536795221, 110.13915686176617, 93.84830850562516, + 80.0820870481205, 71.04848324787652, 68.34789291609472, 72.6765134206983, 83.63977979446459, + 99.77023650435606, 118.77456269522624, 137.94387073801647, 154.60399035259434, 166.49431469790014, + 172.0327438962138, 170.4928109096748, 162.13218328502208, 148.26316136662118, 131.1890050423139, + 113.90461235170372, 99.51589491572973, 90.46166485727544, 87.7622777984477, 90.58353615476729, + 96.33891906881483, 101.3726007035209, 102.04139409749659, 95.84221536672281, 82.19477689643513, + 62.61034213227201, 40.20548872373059, 18.75852748002458, 1.657957295481827, -8.90300742366735, + -12.266306640477236, -9.163238878856461, -1.233167864960895, 9.564495981683649, + 21.43992022175332, 33.051379335207095, 43.56592729978036, 52.568382283902764, 59.9082904825656, + 65.55991544152265, 69.53869459501789, 71.88526118098649, 72.70284621092118, 72.21843649851635, + 70.83368992994872, 69.13827315439758, 67.87334691224184, 67.8505290321987, 69.8442171675916, + 74.47726967536957, 82.11311705302812, 92.75985141769938, 105.99298201503689, 120.91543676261807, + 136.18772657541675, 150.16300634223884, 161.13869607756567, 167.68973106972928, + 168.99857035076562, 165.07365193284676, 156.7707462316869, 145.59585156800276, 133.34673828809346, + 121.70779304048943, 111.92368383650255, 104.63857613545397, 99.9202392953937, 97.42415696314441, + 96.61636301843436, 96.97307879858319, 98.1025495789197, 99.77349073377508, 101.86901694512392, + 104.30470000982287, 106.95197406963027, 109.5967587731362, 111.94423545513219, 113.66135899930364, + 114.4352894158048, 114.02254875294729, 112.27073376540599, 109.10848671943762, 104.5142402049289, + 98.48427593329139, 91.02225206964965, 82.16482996592228, 72.04340823046537, 60.9644383318998, + 49.47596335726721, 38.38208716267078, 28.675265153703773, 21.380090006656577, 17.336295088765645, + 16.98025827773935, 20.1977232072601, 26.30579134436739, 34.1820237918946, 42.50787279716556, + 50.05364436655743, 55.91974464196498, 59.66868428102189, 61.32427965788826, 61.25993059906409, + 60.02892932141973, 58.19725874007958, 56.22498874652909, 54.41596991142811, 52.9291999133868, + 51.82801400185255, 51.138336589289274, 50.892598938155075, 51.146726044509975, 51.96875114225244, + 53.40564661532219, 55.43870808852528, 57.93823519360839, 60.62739916553108, 63.064899619087015, + 64.65655841637202, 64.7055801259163, 62.50676710339701, 57.47927602641731, 49.31661470793777, + 38.11706927966492, 24.4510077185352, 9.330391041091739, -5.928968195692799, -19.927807358089126, + -31.432334046946572, -39.58564206883777, -44.02742340146397, -44.89862410201391, + -42.74448812517229, -38.35747836190584, -32.61145550528262, -26.32954437918894, + -20.20719977067912, -14.789151242435427, -10.48250380540003, -7.582119063177211, + -6.287826913269372, -6.702621306839947, -8.81253570937736, -12.45851173987955, -17.315467703254903, + -22.89273066518226, -28.563759038476075, -33.62421205979971, -37.369384313159216, + -39.17768128465537, -38.587042278968454, -35.35458955341083, -29.49368865852931, + -21.285110785761752, -11.260045417067115, -0.15386876206445768, 11.167509802528333, + 21.80199162922029, 30.914335137198602, 37.83281681446311, 42.1250528920106, 43.6424129336149, + 42.527536031710504, 39.1838083377227, 34.20896823571095, 28.298619788384883, 22.130902226323823, + 16.250738211119412, 10.978234041445447, 6.366580287176825, 2.2267366046830612, -1.780708650958427, + -6.007319377392964, -10.670867462380551, -15.74701218807577, -20.935394624211764, + -25.71475763924228, -29.473174580935297, -31.674804797246836, -32.01284071766054, + -30.503625724620882, -27.49687795069467, -23.603695718045664, -19.567702223835404, + -16.117560259075034, -13.838347771722535, -13.087536620644972, -13.9646827725496, + -16.328965936199715, -19.84970900489466, -24.07290465216959, -28.48977493682292, + -32.59854442331317, -35.95539956520115, -38.2137508958304, -39.15228104770502, -38.692332219912934, + -36.90456601359803, -34.004068329378526, -30.332692841352, -26.327899158206776, + -22.478863588907455, -19.27298194233058, -17.13832959626637, -16.389291255919126, + -17.18279456348231, -19.4913537172405, -23.09696936219348, -27.60749078959273, -32.494672428897246, + -37.15076766150979, -40.95788530079359, -43.36162493187602, -43.93846520775633, + -42.446152023679346, -38.848786666305784, -33.313331043792914, -26.180492183033934, + -17.918310843908614, -9.069298172978261, -0.20074671809499756, 8.136492955152297, + 15.441031108092213, 21.29036758626846, 25.372508549246056, 27.522718958733332, 27.762289494620244, + 26.32911239456066, 23.684885172768034, 20.483910329661786, 17.495388855951955, 15.484035434486861, + 15.069215691030688, 16.59509470023471, 20.047924906971748, 25.048393934050562, 30.927904484071945, + 36.87324018466698, 42.10253497425529, 46.024752154331864, 48.33939153393884, 49.0517871424882, + 48.40580399084933, 46.76081807159332, 44.45507771693884, 41.69825116535199, 38.52289882103032, + 34.803261444171405, 30.327999320714078, 24.898546537842716, 18.420586405729864, + 10.962638364832946, 2.769145218537321, -5.769675941714455, -14.177570162462116, + -21.96287190097139, -28.682775714701705, -33.98993062535793, -37.65744218731808, + -39.584533342726225, -39.78848208739123, -38.38883996706238, -35.58823507210259, + -31.651659030216656, -26.884305680876444, -21.60752145164577, -16.133301088860883, + -10.739449278450719, -5.649094917111724, -1.018762093669203, 3.061876393936713, + 6.558023261640048, 9.468369818928435, 11.800068844557336, 13.547269650304042, 14.679655966520992, + 15.144569931188034, 14.882211925027542, 13.849398415516339, 12.04474239771272, 9.52770863530006, + 6.425839876890219, 2.9278589763920806, -0.735815798162439, -4.320509611419369, -7.596486438809738, + -10.37389619761122, -12.519116524619744, -13.960610192047202, -14.685038419335728, + -14.726561926961596, -14.153639370214792, -13.057964531219815, -11.549353043113381, + -9.758421987878664, -7.846069969532445, -6.015682302494043, -4.521627491850837, + -3.6670759284008367, -3.78622388922754, -5.210529940531722, -8.224269075041521, + -13.019390902549098, -19.661194514880624, -28.07379403356179, -38.04868886403423, + -49.2733259570762, -61.37184021856482, -73.94857243204568, -86.62633925391154, -99.07453382284537, + -111.02550604149512] +pitch: [-157.8684778032057, -158.3698356698477, -159.00269434786534, -159.7713293302798, + -160.66230277415343, -161.6470704856313, -162.69256942436138, -163.77220996055956, + -164.87013794172887, -165.9791087337121, -167.09653844180943, -168.22171491746042, + -169.35450952912657, -170.49493872451055, -171.6430483690157, -172.79888945081638, + -173.96251395289818, -175.13397428130384, -176.31332320177768, -177.50061383591552, + -178.69589966306725, -179.89923452271375, -181.1106726168898, -182.33026851262574, + -183.55807714440536, -184.7941538166402, -186.0385542061607, -187.29133436472392, + -188.5525507215386, -189.8222600858066, -191.10051964928178, -192.38738698884652, + -193.6829200691045, -194.98717724499227, -196.3002172644072, -197.62209927085402, + -198.95288280610868, -200.29262781290038, -201.64139463761154, -202.99924403299593, + -204.36623716091532, -205.7424355950942, -207.12790132389352, -208.52269675310262, + -209.92688470875024, -211.3405284399345, -212.76369162167163, -214.19643835776446, + -215.6388331836894, -217.09094106950369, -218.55282742277168, -220.02455809151112, + -221.50619936715893, -222.99781798755748, -224.49948113996047, -226.0112564640592, + -227.53321205502968, -229.0654164665993, -230.60793871413497, -232.16084827775205, + -233.72421510544342, -235.2981096162307, -236.88260270333552, -238.47776573737366, + -240.08367056956885, -241.70038953498985, -243.3279954558082, -244.96656164457897, + -246.6161619075422, -248.27687054794822, -249.9487623694034, -251.63191267924097, + -253.32639729191183, -255.03229253240016, -256.7496752396611, -258.4786227700819, + -260.2192130009664, -261.9715243340426, -263.73563569899414, -265.5116265570161, + -267.2995769043935, -269.0995672761047, -270.9116787494495, -272.7359929477005, + -274.57259204377965, -276.42155876396043, -278.28297639159297, -280.15692877085615, + -282.04350031053394, -283.9427759878175, -285.85484135213284, -287.77978252899436, + -289.71768622388356, -291.66863972615556, -293.6327309129702, -295.61004825325045, + -297.6006808116674, -299.60471825265194, -301.62225084443344, -303.65336946310623, + -305.69816559672216, -307.7567313494122, -309.82915944553446, -311.91554323385157, + -314.0159766917348, -316.1305544293973, -318.2593716941557, -320.40252437472, -322.5601090055133, + -324.7322227710197, -326.91896351016163, -329.12042972070725, -331.33672056370636, + -333.56793586795794, -335.81417613450594, -338.0755425411669, -340.3521369470865, + -342.64406189732756, -344.9514206274901, -347.27431706835876, -349.61285585057675, + -351.9671423093153, -354.337282488798, -356.72338314601444, -359.1255517507706, + -361.5438964706455, -363.97852609840754, -366.4295497760087, -368.8970760521407, + -371.3812099206837, -373.88204421775976, -376.39963653729615, -378.9339521671109, + -381.48473467467466, -384.0512382159208, -386.631726774158, -389.2226378464643, + -391.81736161291474, -394.40474356794095, -396.9676796187925, -399.48243974549047, + -401.9194238634856, -404.2457230176494, -406.4291524303254, -408.4426925007373, + -410.267999574875, -411.89703489626, -413.33164749245606, -414.5816483484149, -415.66220963815863] +roll: [-157.8684778032057, -158.3698356698477, -159.00269434786534, -159.7713293302798, + -160.66230277415343, -161.6470704856313, -162.69256942436138, -163.77220996055956, + -164.87013794172887, -165.9791087337121, -167.09653844180943, -168.22171491746042, + -169.35450952912657, -170.49493872451055, -171.6430483690157, -172.79888945081638, + -173.96251395289818, -175.13397428130384, -176.31332320177768, -177.50061383591552, + -178.69589966306725, -179.89923452271375, -181.1106726168898, -182.33026851262574, + -183.55807714440536, -184.7941538166402, -186.0385542061607, -187.29133436472392, + -188.5525507215386, -189.8222600858066, -191.10051964928178, -192.38738698884652, + -193.6829200691045, -194.98717724499227, -196.3002172644072, -197.62209927085402, + -198.95288280610868, -200.29262781290038, -201.64139463761154, -202.99924403299593, + -204.36623716091532, -205.7424355950942, -207.12790132389352, -208.52269675310262, + -209.92688470875024, -211.3405284399345, -212.76369162167163, -214.19643835776446, + -215.6388331836894, -217.09094106950369, -218.55282742277168, -220.02455809151112, + -221.50619936715893, -222.99781798755748, -224.49948113996047, -226.0112564640592, + -227.53321205502968, -229.0654164665993, -230.60793871413497, -232.16084827775205, + -233.72421510544342, -235.2981096162307, -236.88260270333552, -238.47776573737366, + -240.08367056956885, -241.70038953498985, -243.3279954558082, -244.96656164457897, + -246.6161619075422, -248.27687054794822, -249.9487623694034, -251.63191267924097, + -253.32639729191183, -255.03229253240016, -256.7496752396611, -258.4786227700819, + -260.2192130009664, -261.9715243340426, -263.73563569899414, -265.5116265570161, + -267.2995769043935, -269.0995672761047, -270.9116787494495, -272.7359929477005, + -274.57259204377965, -276.42155876396043, -278.28297639159297, -280.15692877085615, + -282.04350031053394, -283.9427759878175, -285.85484135213284, -287.77978252899436, + -289.71768622388356, -291.66863972615556, -293.6327309129702, -295.61004825325045, + -297.6006808116674, -299.60471825265194, -301.62225084443344, -303.65336946310623, + -305.69816559672216, -307.7567313494122, -309.82915944553446, -311.91554323385157, + -314.0159766917348, -316.1305544293973, -318.2593716941557, -320.40252437472, -322.5601090055133, + -324.7322227710197, -326.91896351016163, -329.12042972070725, -331.33672056370636, + -333.56793586795794, -335.81417613450594, -338.0755425411669, -340.3521369470865, + -342.64406189732756, -344.9514206274901, -347.27431706835876, -349.61285585057675, + -351.9671423093153, -354.337282488798, -356.72338314601444, -359.1255517507706, + -361.5438964706455, -363.97852609840754, -366.4295497760087, -368.8970760521407, + -371.3812099206837, -373.88204421775976, -376.39963653729615, -378.9339521671109, + -381.48473467467466, -384.0512382159208, -386.631726774158, -389.2226378464643, + -391.81736161291474, -394.40474356794095, -396.9676796187925, -399.48243974549047, + -401.9194238634856, -404.2457230176494, -406.4291524303254, -408.4426925007373, + -410.267999574875, -411.89703489626, -413.33164749245606, -414.5816483484149, -415.66220963815863] +x: [-127.90269211939827, -131.2678097441197, -136.70543077518892, -142.80201308388703, + -147.68698584712374, -150.43090024970653, -151.93731066347436, -153.97975151042488, + -157.1611591307891, -160.11613483598717, -160.9024782819122, -159.04126159161888, + -156.07990100822514, -154.23554261200883, -154.56584426858808, -156.35317090044515, + -158.04548754985666, -158.68146511423873, -158.50567928984265, -158.41585507282485, + -158.95307629614172, -159.79740407636362, -160.1014847915644, -159.23338066405643, + -157.30653996810443, -155.20316918530557, -154.14843270998995, -155.05581305813232, + -157.9836096719787, -162.09393936591388, -166.22381774921027, -169.63236007909416, + -172.26672230472926, -174.36451975460542, -175.90099493885046, -176.49219286857985, + -175.798054128794, -173.93808909631377, -171.4925114649683, -169.12343773356466, + -167.17664041477232, -165.5708829941525, -164.00639105353508, -162.2887264315374, + -160.51670606493755, -159.0215878949382, -158.1431878867084, -158.0223901064516, + -158.5297306199403, -159.33939301424593, -160.09571223386598, -160.59095964674498, + -160.8477855836447, -161.02451272164438, -161.17795428146462, -161.05434333340307, + -160.10141346846075, -157.75281536914142, -153.82556545048772, -148.76090920354517, + -143.52247341799338, -139.17693928388738, -136.36789818536403, -134.95124204564573, + -133.9874074915166, -132.1254263634103, -128.22234525346727, -121.90425650473114, + -113.78832718004412, -105.27070727944219, -98.02475400195476, -93.46091461275282, + -92.30826635434332, -94.3390728032595, -98.26494379864691, -101.94885284908618, + -103.04849210213153, -99.9163743951314, -92.28166421723154, -81.29521224087058, + -68.95793224897206, -57.37532906713539, -48.286218251495455, -42.9311115531321, + -41.97664928415379, -45.25819014572354, -51.473054204178524, -58.23430997331244, + -62.75013392767508, -62.9223262047309, -58.280188016636316, -50.21077109811404, + -41.35681129895958, -34.4990081709638, -31.449867093567427, -32.42754723012978, + -36.15011519371447, -40.58089839438473, -43.94788650904792, -45.50317129106411, + -45.62757380883436, -45.28654859482272, -45.24609917358149, -45.57109091310874, + -45.68440108090419, -44.8673235962936, -42.817117026428, -39.89366389594422, -36.92267974205595, + -34.701751190785245, -33.51756732176649, -32.957648526197254, -32.118390119251, + -30.08038179415433, -26.373159258276946, -21.169894158050823, -15.12315035159766, + -8.966988036828536, -3.135614678740315, 2.3860480980408405, 7.925467603882083, 13.822714648008436, + 20.175371018999993, 26.7822639200393, 33.293134470121245, 39.42975802508738, 45.09936774714966, + 50.30205732126428, 54.88772677310383, 58.342229552134924, 59.78844212222738, 58.2636935332989, + 53.15779898393836, 44.5830880558185, 33.47443622424592, 21.363593342632985, 9.940566867866028, + 0.6026213709298839, -5.8399344345675575, -9.240708782689756, -10.022124423046609, + -8.998133886903553, -7.186216889606415, -5.63851099666342, -5.297145100741871, -6.879790086756265, + -10.809823525239507, -17.202765649327844, -25.90589109754088, -36.57282515330877] +y: [-95.1053320679519, -102.57942707703143, -115.79349366134936, -132.91150346877552, + -149.8018623891795, -161.15737205071278, -162.80068928833558, -153.8389080682052, + -137.30710232263985, -118.90086421096659, -104.39325930921773, -97.04253564148495, + -96.49753527612377, -99.89130313663716, -104.21834166352956, -108.11727712225031, + -111.8967411258529, -116.28298547066109, -121.23895629376239, -125.73427933608644, + -128.36883519321356, -128.19087124969568, -125.13859527382397, -120.00974008025909, + -114.2430140709469, -109.69248942902398, -108.19817332485465, -110.78624497979699, + -116.93512079376003, -124.6998747734192, -131.86718572831617, -137.27226089357666, + -141.20696974324156, -144.759479700215, -148.87001560457742, -153.88261513985736, + -159.68734476449134, -166.07420632507709, -172.96469725505096, -180.3920700150901, + -188.21937883314945, -195.72926730208002, -201.41736840430042, -203.31624017701841, + -199.82553623123383, -190.59338598173065, -176.88869378565937, -161.2238841338048, + -146.45822831996625, -134.87673586346156, -127.65308031796329, -124.80090469016707, + -125.45619356179108, -128.27549428216608, -131.84101844992867, -135.07530215082386, + -137.65337624491679, -140.25913711418366, -144.43087684329745, -151.8938368423789, + -163.67339640117606, -179.56328628213708, -198.32489175456183, -218.42489246839165, + -238.7164724172193, -258.60743976458997, -277.7429960783446, -295.56813001110913, + -311.0788097160963, -322.80675460113724, -328.93908316116415, -327.56037608593266, + -317.12330108413994, -297.14900905724227, -268.84769440086353, -235.16971112565426, + -200.04553112593794, -167.12032560737575, -138.66238790035453, -115.19890389580594, + -95.95279511481895, -79.72820892106228, -65.76755588438334, -54.22790801461051, + -46.13190044244114, -42.84004879066222, -45.24539383402826, -52.99497625151277, + -64.07018690090207, -74.97804538426473, -81.59594832574722, -80.41038856548796, + -69.63949990787273, -49.72242741663517, -22.966995663147852, 7.39427794510237, 38.19309913101399, + 66.89173432062842, 91.64289937473961, 111.13226208974095, 124.5518791391896, 131.77418236143038, + 133.46586276149228, 130.83435645169016, 124.99732180672979, 116.333135612117, 104.26680906118598, + 87.68178840678189, 65.74204569072383, 38.662165827940036, 8.014089907704136, -23.571276834584964, + -53.142563231420375, -78.2522773792973, -97.51345531907658, -110.70725605031265, + -118.5016237829998, -122.00842291412158, -122.404338258332, -120.6995916626668, + -117.59445385917022, -113.3407631971568, -107.62417911491204, -99.58313488352168, + -88.06247336126238, -72.06061544362858, -51.18535335279474, -25.910478578073977, + 2.4559772454057507, 32.01779445834423, 60.591762572708525, 85.94477375005923, 105.9576058260377, + 118.78964738570927, 123.11631379953998, 118.43389190043557, 105.33379726489348, + 85.60032102947753, 62.014359671431286, 37.84509121681435, 16.144470009647492, -0.9347463893425387, + -12.559784399157243, -19.23707231899498, -22.45859920077777, -24.150622679079273, + -26.143960951682608, -29.820707661306084, -35.98068482695456, -44.88212815398885] +yaw: [-157.8684778032057, -158.3698356698477, -159.00269434786534, -159.7713293302798, + -160.66230277415343, -161.6470704856313, -162.69256942436138, -163.77220996055956, + -164.87013794172887, -165.9791087337121, -167.09653844180943, -168.22171491746042, + -169.35450952912657, -170.49493872451055, -171.6430483690157, -172.79888945081638, + -173.96251395289818, -175.13397428130384, -176.31332320177768, -177.50061383591552, + -178.69589966306725, -179.89923452271375, -181.1106726168898, -182.33026851262574, + -183.55807714440536, -184.7941538166402, -186.0385542061607, -187.29133436472392, + -188.5525507215386, -189.8222600858066, -191.10051964928178, -192.38738698884652, + -193.6829200691045, -194.98717724499227, -196.3002172644072, -197.62209927085402, + -198.95288280610868, -200.29262781290038, -201.64139463761154, -202.99924403299593, + -204.36623716091532, -205.7424355950942, -207.12790132389352, -208.52269675310262, + -209.92688470875024, -211.3405284399345, -212.76369162167163, -214.19643835776446, + -215.6388331836894, -217.09094106950369, -218.55282742277168, -220.02455809151112, + -221.50619936715893, -222.99781798755748, -224.49948113996047, -226.0112564640592, + -227.53321205502968, -229.0654164665993, -230.60793871413497, -232.16084827775205, + -233.72421510544342, -235.2981096162307, -236.88260270333552, -238.47776573737366, + -240.08367056956885, -241.70038953498985, -243.3279954558082, -244.96656164457897, + -246.6161619075422, -248.27687054794822, -249.9487623694034, -251.63191267924097, + -253.32639729191183, -255.03229253240016, -256.7496752396611, -258.4786227700819, + -260.2192130009664, -261.9715243340426, -263.73563569899414, -265.5116265570161, + -267.2995769043935, -269.0995672761047, -270.9116787494495, -272.7359929477005, + -274.57259204377965, -276.42155876396043, -278.28297639159297, -280.15692877085615, + -282.04350031053394, -283.9427759878175, -285.85484135213284, -287.77978252899436, + -289.71768622388356, -291.66863972615556, -293.6327309129702, -295.61004825325045, + -297.6006808116674, -299.60471825265194, -301.62225084443344, -303.65336946310623, + -305.69816559672216, -307.7567313494122, -309.82915944553446, -311.91554323385157, + -314.0159766917348, -316.1305544293973, -318.2593716941557, -320.40252437472, -322.5601090055133, + -324.7322227710197, -326.91896351016163, -329.12042972070725, -331.33672056370636, + -333.56793586795794, -335.81417613450594, -338.0755425411669, -340.3521369470865, + -342.64406189732756, -344.9514206274901, -347.27431706835876, -349.61285585057675, + -351.9671423093153, -354.337282488798, -356.72338314601444, -359.1255517507706, + -361.5438964706455, -363.97852609840754, -366.4295497760087, -368.8970760521407, + -371.3812099206837, -373.88204421775976, -376.39963653729615, -378.9339521671109, + -381.48473467467466, -384.0512382159208, -386.631726774158, -389.2226378464643, + -391.81736161291474, -394.40474356794095, -396.9676796187925, -399.48243974549047, + -401.9194238634856, -404.2457230176494, -406.4291524303254, -408.4426925007373, + -410.267999574875, -411.89703489626, -413.33164749245606, -414.5816483484149, -415.66220963815863] +z: [27.487690876753174, 15.337246293823744, -6.519501713577381, -32.17378390020878, + -52.95176670190418, -61.55800766961198, -55.94481745228971, -40.02071700799717, + -21.554360535385083, -9.07142702001839, -8.346860847184745, -19.368396082981224, + -36.19083663458397, -50.83073806660286, -58.44885149238343, -59.53297302408236, + -57.86810718901493, -56.968343215030046, -58.092142359329614, -60.51600905693699, + -62.7444480158786, -63.35926464089802, -61.25799874825209, -55.70616111817719, -46.57263622352219, + -34.71402685096095, -22.12817933348742, -11.450779533332875, -4.771195360275572, + -2.4341668275669868, -2.815083023486069, -3.436788885507724, -2.639407740293576, + -0.49257916688823505, 1.6634057300540754, 2.4787157173774994, 1.5816568083905458, + -0.3081254457303777, -1.9465496236895794, -2.235946638392112, -0.587599350222585, + 3.0830448889107243, 8.585148043725034, 15.741871127579394, 24.588070885472288, 35.317967966866895, + 47.92047635319786, 61.70329222579224, 75.14570192551099, 86.40468533274036, 94.26512130476762, + 98.82097106678889, 101.29642767567256, 103.1652314081855, 105.33907861244916, 107.99131029859556, + 110.83900010952557, 113.33429248514499, 114.58664344276714, 113.36413234926317, + 108.50758731712286, 99.61208997647628, 87.50344465477376, 74.1768095655181, 62.22532988939497, + 54.03291376712356, 51.07089312083894, 53.5800909155043, 60.73095402033792, 71.08141473731635, + 83.00300927492155, 94.88248494409528, 105.19436856609353, 112.66234904016706, 116.5532902732588, + 116.89064945934301, 114.34290812223043, 109.7932211178687, 103.88036334169092, 96.83894755858785, + 88.72518958612778, 79.81089200833654, 70.81116373157523, 62.753896533262505, 56.569823287756314, + 52.671108932076606, 50.767905449313446, 49.98887184490788, 49.18533829409316, 47.24706022687302, + 43.33944398884515, 37.069957898660974, 28.60206393388892, 18.673743747050306, 8.448948216774712, + -0.7972607166725888, -8.036081276408424, -12.709833208187701, -14.735329114490181, + -14.3542414036342, -11.989651298267246, -8.214474065551915, -3.796221322329577, + 0.3127831108565613, 3.1349675066051397, 3.941404446156438, 2.4944463514526865, -0.8862482050049294, + -5.474872423279538, -10.422540767592642, -15.02656078493345, -18.872907062153832, + -21.8268990816907, -23.925876718830928, -25.25373825675896, -25.85191903924253, + -25.679848344814015, -24.61517234703336, -22.486711991951278, -19.140322650202826, + -14.528471985404261, -8.792284878821462, -2.2948582536620714, 4.415692468816276, + 10.697174253055142, 15.917112597767035, 19.536456023535326, 21.170365945226813, + 20.644086856810723, 18.052446724627877, 13.800892715061188, 8.584501211461651, 3.2736044258695483, + -1.2815934000814575, -4.466744501254597, -6.057544821897366, -6.251111895303721, + -5.565299343444047, -4.652409384326363, -4.10510631771834, -4.32579622671807, -5.493001584407323, + -7.61115583178264, -10.59834849765868, -14.363422130581966, -18.8448260745628, -24.011142744024184, + -29.84051753345265, -36.298155831550744, -43.32322171459229] diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/config/trajectory_weights/weights_table_grasp2.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/config/trajectory_weights/weights_table_grasp2.yaml deleted file mode 100644 index eb2ff09af..000000000 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/config/trajectory_weights/weights_table_grasp2.yaml +++ /dev/null @@ -1,205 +0,0 @@ -pitch: [307.4878359078107, 268.6905664663287, 184.8643455969408, 58.545770552115364, - -89.56664765402549, -224.0840865697819, -311.65407200869106, -340.4518465053893, - -326.36988658361236, -301.0968996723944, -291.5230956032813, -302.448851382407, - -308.3914210364421, -257.0395550286017, -88.99193040972278, 224.31037039483945, - 637.5643785752965, 1014.2632003322818, 1164.60022439979, 937.0932511424448, 318.12976917975465, - -527.5153035415188, -1314.8532609032436, -1771.4497973469677, -1766.3703806713406, - -1356.8367650999844, -731.5592528971431, -99.4581862321175, 399.86183644411614, - 718.1307829590074, 864.2303274362567, 860.5931414552338, 727.3067660910908, 493.74315683086445, - 211.0550064061407, -55.45330435442286, -250.34727987639022, -348.6866563291638, - -358.565639352564, -307.32677689486184, -224.36311412190383, -131.82405445887937, - -44.96270554146841, 23.964988907092717, 64.72417513759937, 72.30507366754365, 50.61763777504188, - 12.362262387723732, -26.009558772221443, -51.563046820900915, -60.25116047362398, - -57.63900635128191, -54.959634616138075, -62.76006262040929, -85.28131693354047, - -118.1619302012079, -150.44250732835803, -169.7791822109458, -168.1441788914054, - -145.07303803275605, -107.02233887068444, -63.74432140905734, -24.160657132225335, - 6.0201500194586295, 24.353211429945322, 30.418517152171734, 24.744807885237314, - 8.54681746977837, -16.072008864403312, -46.073491254692094, -77.77427601735346, - -107.2115322777933, -130.4200732695773, -143.8634527292192, -145.35651564226546, - -135.32974372376032, -117.6277916325565, -98.8975621416649, -86.30310639836785, - -84.41064317127606, -92.8345065874572, -106.05233844759589, -115.72596830250339, - -114.51044215834928, -99.48168494803396, -73.46438430348987, -43.629996980997795, - -18.156763095107696, -2.6983372116195183, 1.6251365457887872, -2.0597324324561095, - -8.49567377463034, -12.953091059582318, -13.113183257935408, -9.481331616425296, - -4.45900774984968, -0.7848834778791399, -0.18676565696596104, -2.808904223949428, - -7.5024500572913935, -12.643567728048698, -16.95955800706378, -19.94186321219478, - -21.708274735805453, -22.481173409023892, -22.036303473716234, -19.483798588997214, - -13.578418452496079, -3.4826781595599914, 10.366639194007917, 25.77702234203514, - 39.091379046783125, 46.29788026101588, 44.56751406543836, 33.53582040720307, 15.694765808949526, - -4.334486259581232, -21.323797450096354, -31.041584270219868, -31.22069213125181, - -21.54964734625588, -2.8776138683103407, 23.7844367985862, 57.82513782104277, 98.79780481615855, - 145.4992270312695, 194.90101264523364, 241.65465388393397, 278.7147855690786, 299.1252817673519, - 298.39336513620555, 276.45014679106646, 238.23150408778133, 192.46066874693108, - 149.0289783101666, 116.02858010020579, 97.63386102982291, 93.58901641305495, 100.29361472348529, - 112.7990857264626, 126.76046188006744, 139.60001029106323, 150.64811215552695, 160.52595461631785, - 170.27880196415424, 180.70091624441704, 192.04307822110903, 204.05305965949202, - 216.18527381768172, 227.82687947071088] -roll: [209.26310319076802, 185.35708614868105, 132.6897487870579, 54.74474038999782, - -31.519454510078013, -98.13006804695982, -118.77669951323375, -85.06693497694505, - -14.235704420919252, 58.16814298294952, 96.67724083087974, 85.15954851222429, 36.873829392237596, - -10.145406945028034, -11.481257116039632, 58.52851156001803, 186.3263326311764, - 315.50269174000914, 366.84348407733404, 276.95027122405827, 36.73874124450958, -294.0894554520116, - -609.7059109083152, -807.7918688909466, -836.3079077346105, -708.6679360172418, - -481.9615748339739, -219.52651695828882, 33.523250870445345, 249.79894425175672, - 405.80675450006413, 475.8414790211664, 443.1339326191075, 317.9892223406249, 143.97679712422791, - -16.929647559130625, -111.72437322217338, -118.54462373871019, -51.16597816395034, - 53.87450880811503, 157.2424781910211, 231.9761739730581, 267.7723038745797, 266.3475125713114, - 234.5635176694967, 180.88439377517432, 115.78806223060508, 52.645730705883835, 5.590726742336357, - -15.6733964645854, -10.816947889478474, 10.163304297425586, 31.218593642664974, - 37.70081170101127, 23.116695718718013, -8.20667536030345, -43.686190491276086, -68.69649557045352, - -73.06837593120727, -54.64085691395713, -18.63246623935183, 25.850445993994438, - 69.51072010097609, 104.80830847584399, 126.12959487616133, 129.57146050260752, 113.55161699310824, - 80.07737649246528, 35.35586720125649, -11.387698416925671, -50.68753776186014, -75.75959843259228, - -84.02293625033323, -76.73566097281713, -57.494444226847854, -30.760409155652727, - -1.0276326529813191, 27.562102578316143, 51.662531378506465, 69.37983248353137, - 80.7684289277017, 87.628045221175, 92.41011548202059, 96.61452101767676, 99.47051698157318, - 97.71676906600825, 86.88901059775175, 63.80375285151239, 29.164901084806242, -11.160013096117748, - -47.262641207400634, -68.63171822218054, -68.46417700105185, -46.71376113942381, - -10.27127461618544, 29.81541870972953, 62.61275158048216, 80.95721075519452, 82.98199830641752, - 71.3139477529786, 50.89179151297815, 26.739883740545775, 2.6378917734857015, -19.10301168403571, - -37.1278851161751, -50.36456315285917, -57.64841632980128, -57.843181803673154, - -50.38294932729228, -35.94466489294901, -16.857642838149165, 3.0719474036396037, - 19.39001696071014, 28.21647015844554, 27.38317791547756, 17.046417831149483, -0.4042222480479882, - -21.00352901558579, -40.12681572356702, -53.39478061329125, -57.446634329655396, - -50.56130059021794, -33.08140703270382, -7.510664287348054, 21.85416673618458, 49.76342142065035, - 71.12837322282269, 82.07761137185807, 80.65960322544773, 67.06414330741778, 43.41422704198978, - 13.263606774566217, -19.081626061668885, -49.34020699117489, -74.0478926740234, - -91.22824282120375, -100.7717211846692, -104.3978188758738, -105.19551733743343, - -106.87164934337899, -112.91706829452616, -125.90116577727926, -147.04455533292247, - -176.13073203863172, -211.72608421627854, -251.60421759783222, -293.23533244875233, - -334.21463552288907, -372.55390356472765, -406.81959736322625] -x: [-45.06172651446696, -44.50528599867761, -42.368640652908475, -38.830535456218065, - -34.65801579069439, -30.983082543826082, -28.72202872009684, -28.015528300878216, - -28.085991021330404, -27.58455176054732, -25.262499727416778, -20.75564499359652, - -15.19536087941695, -11.21671003700053, -11.993639360021191, -19.437422277745792, - -32.447943386643736, -46.478480540755164, -55.149776956285436, -53.3934966463505, - -40.445788116615965, -20.825459731577343, -2.477606887905092, 7.035867486206661, - 4.155856374674821, -9.303862131425433, -27.657692673971805, -44.546343814865175, - -55.7689455322009, -60.25261140134869, -59.13712408519084, -54.160813823309184, - -46.62111236408496, -37.305646841577804, -26.893927075314252, -16.183920442030807, - -5.999674997697829, 2.910908698387282, 9.755343424645364, 13.538283066156557, 13.241472976673812, - 8.396625694314823, -0.38079952591690547, -11.397119171484686, -22.45507255244106, - -31.52321272828289, -37.03035385669701, -37.80041338037171, -33.0712566283519, -22.95158885195406, - -9.124585729331447, 4.883204007262881, 14.480520566178551, 15.770177571956838, 7.41293334710724, - -8.552445864978143, -27.5313642341668, -44.41663657860791, -55.68388034904888, -60.427741680611774, - -59.9937930459423, -56.719364192122605, -52.65005273653141, -48.83977228724342, - -45.33404050933557, -41.59016220917724, -37.02088588471639, -31.426464347551594, - -25.173924942450824, -19.08264482088237, -14.101403266516412, -10.961627261981635, - -9.981518914691089, -11.072260440425637, -13.861239803818183, -17.80909929942429, - -22.270666373919425, -26.54450321960095, -29.975253902604027, -32.107259961129884, - -32.812067748569234, -32.30360716444636, -31.0201553767969, -29.433007073990666, - -27.87503588394257, -26.453081217503584, -25.050397747158176, -23.38476177482476, - -21.085517394485183, -17.77776385282289, -13.185562167890449, -7.263320555902968, - -0.3294708406457369, 6.869419554481178, 13.250421173333713, 17.6760018421937, 19.351189241920277, - 18.163394476378677, 14.785784235499513, 10.47501844204813, 6.655439819182812, 4.485718819454019, - 4.58549847944645, 6.986206011304477, 11.250711135388523, 16.65542375999597, 22.355100377099344, - 27.508920811058278, 31.383724878043846, 33.44881037637206, 33.45614748643667, 31.48873627610872, - 27.96619536639039, 23.605724547672324, 19.33357060504991, 16.133879748033763, 14.832217030260278, - 15.853115457495218, 19.04410095139672, 23.67625043538951, 28.677693300936625, 33.04210297910169, - 36.239788348955784, 38.42043366054024, 40.27448445421512, 42.58813523027747, 45.700060492278425, - 49.14846003137164, 51.72880324371295, 51.98688602021415, 48.941383357875814, 42.68581046653293, - 34.54746197388396, 26.68093213691205, 21.252995525236017, 19.590579912456967, 21.696830762957248, - 26.367017650691324, 31.841655780072134, 36.67100305173607, 40.37149025572581, 43.57957874812686, - 47.68005182843892, 54.14666611922619, 63.93627298937479, 77.18452867591833, 93.25272676218941, - 111.008895420734, 129.17116244093623, 146.58353986331068] -y: [-25.632812653878084, -25.667026850630098, -25.65332690812688, -25.558627745804298, - -25.33186876288174, -24.904897968381164, -24.226218075506107, -23.346949242002584, - -22.525712735071995, -22.251250918985015, -23.085902930377973, -25.359151452629384, - -28.90513928701911, -33.066260971831575, -36.99637792168402, -40.03826105838443, - -41.880735628177405, -42.42050800341945, -41.5567807229203, -39.21066023803674, - -35.599482619495845, -31.47667698045251, -28.009473562646896, -26.25678941493762, - -26.55439421679798, -28.216585252720197, -29.77202618579056, -29.64711497611054, - -26.986915393790145, -22.222325752189317, -17.04789312298538, -13.702191827969843, - -13.833811699034209, -17.575801405395516, -23.413990272727666, -28.95024708198037, - -32.0831960812367, -31.909070477121002, -28.903841440489884, -24.4268833368915, - -19.944130223749177, -16.425839500414593, -14.161649030460561, -12.929616029397886, - -12.266096677317192, -11.61826685907862, -10.35404729487928, -7.780517167794712, - -3.3477180847008396, 2.9236071062312, 9.976564391358705, 15.58103577341059, 16.85974444144582, - 11.469939519433398, -1.079396478715883, -18.786351219778194, -37.73281968864431, - -53.67606253742803, -63.7665596338745, -67.43044993864187, -66.05735789009529, -61.91288732437796, - -57.04711768075798, -52.72981087096425, -49.45062439775582, -47.21155679269143, - -45.83821983372657, -45.182047343603166, -45.1943468982154, -45.8897019743006, -47.23411283167097, - -49.02109598560663, -50.81643091894815, -52.03137069957861, -52.121037130400474, - -50.825141958817056, -48.31933867824606, -45.17065043113971, -42.090592529998936, - -39.598662297423516, -37.768709649386004, -36.19087119160269, -34.16819186445556, - -31.048004963271847, -26.526682203483297, -20.789139245486016, -14.435796157768074, - -8.259559885376913, -2.999964631715642, 0.8186650166284414, 2.9422761080995, 3.387415256575487, - 2.4420763596642288, 0.6635327469620856, -1.179934294458653, -2.2590851149220046, - -1.8911451779088735, 0.28561294065438286, 4.251437691967593, 9.675710506169587, - 16.06681022211528, 22.921733737168566, 29.81746909367072, 36.43167509245798, 42.51653506067146, - 47.859365580737574, 52.25240203792988, 55.479350852962426, 57.319328935856134, 57.56909136480433, - 56.08566795047459, 52.849164358239534, 48.03715133047759, 42.08664925098448, 35.702580137057076, - 29.76916997469084, 25.151441007074254, 22.436448222940378, 21.725577938714736, 22.6016034969853, - 24.329513221147273, 26.229211741871882, 28.045518140109163, 30.10798162753032, 33.15425242231318, - 37.858495287271715, 44.27968744192287, 51.52751426199087, 57.87543900348522, 61.34545269040426, - 60.539929869686446, 55.33441340375058, 47.07099830279992, 38.11135947152029, 30.919291820345794, - 27.083951997907352, 26.735370322450763, 28.616832132079118, 30.75971816520821, 31.42066844205043, - 29.834764123115985, 26.46114693907252, 22.67392658476609, 20.12175806668451, 20.095614133885213, - 23.164629534288462, 29.149928104993194, 37.34013985555722, 46.789986502918026, 56.57574237593195] -yaw: [-85.82284184234118, -81.26044783480269, -70.07101489342415, -52.73568112770571, - -32.312887926707255, -13.78839906939816, -1.6672223628687894, 2.62855477579123, - 1.4637997318473883, -0.7122178494319122, -0.2890683255906193, 2.770600737662801, - 3.2517524724015923, -9.01552308198881, -45.45607518744383, -111.05972990835949, - -195.39715744012742, -268.8330368320655, -290.4412975641661, -227.7732085861161, - -78.9317597006005, 117.9201686381322, 297.2293991423197, 396.33374712240726, 385.71000473625224, - 280.50998160917317, 127.53082352544902, -21.591635655970986, -132.8294537852815, - -196.17596093309243, -217.6704538759289, -207.65930075811795, -174.68571913075345, - -126.36479441762415, -72.15015673522322, -23.100178225311307, 11.58771751528709, - 27.978929943856027, 27.906619220099845, 16.62722814185456, -0.16994623726551647, - -17.947750597398596, -33.17934811581, -42.66989953652374, -43.643970253947295, -34.82553827018641, - -17.59531893754674, 3.929234637048963, 24.263557047593597, 38.64014574629869, 44.552947689726544, - 41.97514116975504, 32.47694516434011, 18.147714201342975, 1.0543883277121482, -16.755211464052067, - -33.21729388374368, -46.57313523562773, -55.90724331833623, -61.43109318991602, - -64.21217824353955, -65.48793688068338, -65.9705207973369, -65.47926190899054, -63.023571617961, - -57.305023318747764, -47.494327619576445, -33.97700167313159, -18.641436579684857, - -4.416663663991963, 5.814384906636482, 10.373847070406663, 9.38411923429142, 4.508860698209896, - -1.9356865197155904, -7.9606412898217584, -12.499124266041337, -15.456913061839213, - -17.37269370009648, -18.966954306399614, -20.79261413744074, -23.053534020790096, - -25.54353654187318, -27.634858828262228, -28.303928178285414, -26.267104900850885, - -20.325567487838626, -9.923911719253681, 4.266053071819271, 20.08528752116726, 34.2619072645698, - 43.435630478396426, 45.44194217842096, 40.22939648971534, 29.921002335698567, 17.985046686760153, - 7.955705382085571, 2.329433852600685, 2.0711572030452037, 6.771528965996401, 15.179522806472088, - 25.75856576877883, 37.04824578537782, 47.805604932006396, 57.016629050400816, 63.87734670108889, - 67.79620197389316, 68.42669261663922, 65.72118040843198, 59.987052835925, 51.912611203184056, - 42.52413442506399, 33.05511088091003, 24.747749762642485, 18.636166646439452, 15.358295662629935, - 15.020899007568389, 17.13149788589931, 20.62788661787804, 24.055160585938665, 25.916901012504752, - 25.143507855405588, 21.51583036210249, 15.833945868198665, 9.689157721402468, 4.8723748326537475, - 2.647898968595296, 3.2228179980864446, 5.6760220781836415, 8.397691363657867, 9.837649261200632, - 9.20442163232231, 6.78138527862297, 3.719218222006543, 1.4229609534044665, 0.843888753429656, - 2.0177255310178945, 4.054148799738449, 5.555057476845689, 5.23807333755755, 2.464326517244721, - -2.5545874557537833, -8.917245787438091, -15.429489147674806, -21.024424026656018, - -25.033276121438494, -27.24968793325762, -27.83529269443636, -27.158644596793252, - -25.645380324756296] -z: [-22.62081372627218, -22.458849389589915, -22.340158902554457, -22.281688493304806, - -22.365061136407927, -22.783008243658642, -23.821075585958067, -25.770596214499594, - -28.86135296108498, -33.26544631579463, -39.04450507004138, -45.82952846645735, - -52.269913587374774, -55.75656315934748, -53.11089966848031, -42.385828263327085, - -24.86439162240813, -5.690258819702291, 7.8631504484666515, 9.877877168383415, -0.999054547295284, - -20.69272715480139, -41.777828295400504, -57.16709867072815, -63.12578232701807, - -59.99447638330691, -50.70475895168664, -38.57061194894658, -25.942421961251238, - -14.240779412656495, -4.664750489376498, 1.5018154527950844, 3.4004229646525492, - 1.3796678726375649, -2.808105554001277, -6.687767274410187, -8.13567855498191, -6.12884439030572, - -0.8262043451300531, 6.859440309828449, 15.802448405523107, 24.98580870233531, 33.52084350515457, - 40.57108640254275, 45.49546788702716, 48.30222273074625, 50.098423905112, 53.01632467678541, - 59.32535943323446, 69.9550778783712, 83.1403276665768, 94.09900368394518, 96.37900956455387, - 84.68921419948347, 57.95847304344338, 20.802149053089696, -17.803763987698026, -48.196705532248124, - -64.05334258054177, -64.52539583557882, -53.530984240916666, -37.054880133149666, - -20.28588637790632, -6.037502459403527, 5.236621851488619, 14.429454803492744, 22.583630638227632, - 30.059855067194608, 36.28274948376065, 40.048521484853396, 40.21129183479851, 36.41448004839188, - 29.48282895280681, 21.241935629479723, 13.846174199316279, 8.976443064217197, 7.319972032589732, - 8.539450931482968, 11.640541057875641, 15.464016816725664, 19.052580077895975, 21.801831228771135, - 23.45521829127891, 24.04836839944462, 23.85682801458122, 23.328355738533755, 22.95662251530825, - 23.093880115620454, 23.768324190695665, 24.609911144877767, 24.959281436177797, - 24.14588012687881, 21.821813902415503, 18.18841686850797, 13.989837373210776, 10.260083272073038, - 7.937434923165615, 7.5282945420541525, 8.969263320855546, 11.723481371537021, 15.02821752987984, - 18.1572033715646, 20.59278751893711, 22.079809495801744, 22.595320729800406, 22.283397926577717, - 21.382969371261755, 20.15189707836302, 18.78636053302327, 17.349778804688402, 15.741082327600221, - 13.729365865020657, 11.056538184639097, 7.574073974976007, 3.3554640075065287, -1.2707331581300847, - -5.789331729992047, -9.643806298237173, -12.399137384313326, -13.859913192763814, - -14.109234743701673, -13.471860394852694, -12.428508393514761, -11.508568062439753, - -11.177408307261295, -11.730428541239489, -13.215270504704932, -15.414360669766424, - -17.913581022513053, -20.25175587304091, -22.102247846902927, -23.40855647511527, - -24.403679701111763, -25.492052525849765, -27.042150597618175, -29.190856906328417, - -31.765243272451322, -34.37489458295107, -36.64080857906956, -38.44975246960033, - -40.10005022850334, -42.254614527413, -45.716546956729246, -51.13480418179141, -58.77622666461046, - -68.45192752900248, -79.59962098381037, -91.456567994907, -103.24154647862528, -114.28940218794558] diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/package.xml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/package.xml index 6e75ac03b..837831a68 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/package.xml +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/package.xml @@ -25,7 +25,7 @@ rosplan_dispatch_msgs rosplan_knowledge_msgs diagnostic_msgs - mcr_perception_msgs + mas_perception_msgs mdr_rosplan_interface mdr_move_arm_action mdr_move_base_action @@ -43,7 +43,7 @@ rosplan_dispatch_msgs rosplan_knowledge_msgs diagnostic_msgs - mcr_perception_msgs + mas_perception_msgs mdr_rosplan_interface mdr_move_arm_action mdr_move_base_action diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/action/Pickup.action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/action/Pickup.action index d6001c17a..2d4f8803d 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/action/Pickup.action +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/action/Pickup.action @@ -1,5 +1,9 @@ # goal +uint32 SIDEWAYS_GRASP=0 +uint32 TOP_GRASP=1 + geometry_msgs/PoseStamped pose +uint32 strategy --- # result bool success diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/launch/pickup_client.launch b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/launch/pickup_client.launch index 995490d51..b00d629a6 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/launch/pickup_client.launch +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/launch/pickup_client.launch @@ -2,8 +2,6 @@ - - @@ -11,8 +9,6 @@ - - diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_action index 78ffb221a..2ca28cd23 100755 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_action +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_action @@ -1,22 +1,41 @@ #!/usr/bin/env python import rospy -import smach - -from smach_ros import ActionServerWrapper, IntrospectionServer +import actionlib from mdr_pickup_action.msg import PickupAction -from mdr_pickup_action.action_states import (SetupPickup, Pickup, SetActionLibResult) +from mdr_pickup_action.action_states import PickupSM +class PickupServer(object): + '''A server exposing a pickup action. -class PickupSkill(smach.StateMachine): - def __init__(self): - smach.StateMachine.__init__(self, - outcomes=['OVERALL_SUCCESS', - 'OVERALL_FAILED', 'PREEMPTED'], - input_keys=['pickup_goal'], - output_keys=['pickup_feedback', - 'pickup_result']) + The server expects the following parameters to be made available on the ROS parameter server: + * move_arm_server: Name of the move_arm action server (default: 'move_arm_server') + * move_base_server: Name of the move_base action server (default: 'move_base_server') + * gripper_controller_pkg_name: The name of a package that implements functionalities for + controlling a robot's gripper (default: 'mdr_gripper_controller') + * pregrasp_config_name: The name of the pregrasp configuration (default: 'pregrasp') + * intermediate_grasp_offset: An optional pose offset that creates an intermediate + trajectory goal following the pregrasp configuration (default: -1) + * safe_arm_joint_config: The name of a configuration in which the robot can + safely move around the environment (default: 'folded') + * base_elbow_offset: An optional offset between base_link and the manipulator's elbow; + used for aligning the base with the object to be grasped + so that the manipulator can easily reach the object (default: -1) + * grasping_dmp: Path to a YAML file containing the weights of a dynamic motion primitive + used for grasping (default: '') + * dmp_tau: The value of the temporal dynamic motion primitive parameter (default: 1) + * grasping_orientation: For more constrained manipulators, it might make sense to use + a fixed grasping orientation (expressed as an (x, y, z, w) quaternion) + to ensure easier reachability (default: [], in which case + the argument is ignored) + * number_of_retries: Number of times a grasp should be repeated in case + it fails the first time (default 0) + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de + + ''' + def __init__(self): move_arm_server = rospy.get_param('~move_arm_server', 'move_arm_server') move_base_server = rospy.get_param('~move_base_server', 'move_base_server') gripper_controller_pkg_name = rospy.get_param('~gripper_controller_pkg_name', @@ -25,56 +44,48 @@ class PickupSkill(smach.StateMachine): grasp_offset = float(rospy.get_param('~intermediate_grasp_offset', -1.)) safe_arm_joint_config = rospy.get_param('~safe_arm_joint_config', 'folded') base_elbow_offset = float(rospy.get_param('~base_elbow_offset', -1.)) + arm_base_offset = float(rospy.get_param('~arm_base_offset', -1.)) grasping_orientation = rospy.get_param('~grasping_orientation', list()) grasping_dmp = rospy.get_param('~grasping_dmp', '') dmp_tau = float(rospy.get_param('~dmp_tau', 1.)) number_of_retries = int(rospy.get_param('~number_of_retries', 0)) - with self: - smach.StateMachine.add('SETUP_PICKUP', SetupPickup(), - transitions={'succeeded': 'PICKUP', - 'failed': 'SETUP_PICKUP'}) + rospy.loginfo('[pickup] Initialising state machine') + self.action_sm = PickupSM(move_arm_server=move_arm_server, + move_base_server=move_base_server, + gripper_controller_pkg_name=gripper_controller_pkg_name, + pregrasp_config_name=pregrasp_config_name, + intermediate_grasp_offset=grasp_offset, + safe_arm_joint_config=safe_arm_joint_config, + base_elbow_offset=base_elbow_offset, + arm_base_offset=arm_base_offset, + grasping_orientation=grasping_orientation, + grasping_dmp=grasping_dmp, + dmp_tau=dmp_tau, + number_of_retries=number_of_retries) + rospy.loginfo('[pickup] State machine initialised') - smach.StateMachine.add('PICKUP', Pickup(move_arm_server=move_arm_server, - move_base_server=move_base_server, - gripper_controller_pkg_name=gripper_controller_pkg_name, - pregrasp_config_name=pregrasp_config_name, - intermediate_grasp_offset=grasp_offset, - safe_arm_joint_config=safe_arm_joint_config, - base_elbow_offset=base_elbow_offset, - grasping_orientation=grasping_orientation, - grasping_dmp=grasping_dmp, - dmp_tau=dmp_tau, - number_of_retries=number_of_retries), - transitions={'succeeded': 'SET_ACTION_LIB_SUCCESS', - 'failed': 'SET_ACTION_LIB_FAILED'}) - - smach.StateMachine.add('SET_ACTION_LIB_FAILED', - SetActionLibResult(False), - transitions={'succeeded': 'OVERALL_FAILED'}) - - smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', - SetActionLibResult(True), - transitions={'succeeded': 'OVERALL_SUCCESS'}) + self.action_server = actionlib.SimpleActionServer('pickup_server', + PickupAction, self.execute, False) + self.action_server.start() + rospy.loginfo('pickup action server ready') + def execute(self, goal): + rospy.loginfo('[pickup] Received an action request') + self.action_sm.goal = goal + self.action_sm.result = None + self.action_sm.execution_requested = True + while not self.action_sm.result: + rospy.sleep(0.05) + self.action_server.set_succeeded(self.action_sm.result) if __name__ == '__main__': rospy.init_node('pickup_server') - sm = PickupSkill() - sis = IntrospectionServer('pickup_smach_viewer', sm, - '/PICKUP_SMACH_VIEWER') - sis.start() - - asw = ActionServerWrapper( - server_name='pickup_server', - action_spec=PickupAction, - wrapped_container=sm, - succeeded_outcomes=['OVERALL_SUCCESS'], - aborted_outcomes=['OVERALL_FAILED'], - preempted_outcomes=['PREEMPTED'], - goal_key='pickup_goal', - feedback_key='pickup_feedback', - result_key='pickup_result') - - asw.run_server() - rospy.spin() + pickup_server = PickupServer() + try: + pickup_server.action_sm.run() + while pickup_server.action_sm.is_running and not rospy.is_shutdown(): + rospy.spin() + except (KeyboardInterrupt, SystemExit): + print('{0} interrupted; exiting...'.format(pickup_server.action_sm.name)) + pickup_server.action_sm.stop() diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_client b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_client index f46d4a264..aa9d73460 100755 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_client +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_client @@ -3,16 +3,18 @@ import rospy import actionlib import tf -from geometry_msgs.msg import PoseStamped -import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import rosplan_knowledge_msgs.srv as rosplan_srvs -import diagnostic_msgs.msg as diag_msgs - -from mcr_perception_msgs.msg import Object +from mas_perception_msgs.msg import Object from mdr_rosplan_interface.action_client_base import ActionClientBase +from mas_knowledge_base.knowledge_base_interface import KBException from mdr_pickup_action.msg import PickupAction, PickupGoal class PickupClient(ActionClientBase): + '''Defines a client for an object pickup action. + + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de + + ''' def __init__(self): super(PickupClient, self).__init__() @@ -35,19 +37,21 @@ class PickupClient(ActionClientBase): client = actionlib.SimpleActionClient(self.action_server_name, PickupAction) client.wait_for_server() goal = self.get_action_message(msg) - - # calling the actionlib server and waiting for the execution to end - rospy.loginfo('[PICKUP] Sending actionlib goal to %s' % self.action_server_name) - client.send_goal(goal) - client.wait_for_result(rospy.Duration.from_sec(int(self.action_timeout))) - result = client.get_result() - - if result and result.success: - rospy.loginfo('[PICKUP] Updating the knowledge base') - self.update_knowledge_base() - self.send_action_feedback(True) + if goal: + # calling the actionlib server and waiting for the execution to end + rospy.loginfo('[pickup] Sending actionlib goal to %s' % self.action_server_name) + client.send_goal(goal) + client.wait_for_result(rospy.Duration.from_sec(int(self.action_timeout))) + result = client.get_result() + + if result and result.success: + rospy.loginfo('[PICKUP] Updating the knowledge base') + self.update_knowledge_base() + self.send_action_feedback(True) + else: + self.send_action_feedback(False) else: - self.send_action_feedback(False) + rospy.loginfo('[pickup] The action dispatch message could not be created; ignoring request') def get_action_message(self, rosplan_action_msg): '''Reads the action parameters and uses them to initialise an actionlib message. @@ -61,73 +65,31 @@ class PickupClient(ActionClientBase): elif param.key == 'bot': self.robot_name = param.value - object_pose = self.get_object_pose(self.obj) - object_pose.header.stamp = rospy.Time(0) - - goal.pose.header.frame_id = self.frame_id - goal.pose.header.stamp = rospy.Time.now() - object_pose_in_grasp_frame = self.tf_listener.transformPose(self.frame_id, - object_pose) - goal.pose = object_pose_in_grasp_frame - return goal - - def get_object_pose(self, object_name): try: - obj = self.msg_store_client.query_named(object_name, Object._type)[0] - return obj.pose - except: - rospy.logerr('Error retriving knowledge about %s', object_name) - return PoseStamped() + obj = self.kb_interface.get_obj_instance(self.obj, Object._type) + object_pose = obj.pose + object_pose.header.stamp = rospy.Time(0) + + goal.pose.header.frame_id = self.frame_id + goal.pose.header.stamp = rospy.Time.now() + object_pose_in_grasp_frame = self.tf_listener.transformPose(self.frame_id, + object_pose) + goal.pose = object_pose_in_grasp_frame + return goal + except KBException as exc: + rospy.logerr('[pickup] %s', str(exc)) + return None def update_knowledge_base(self): - # we remove the fact that the object is on the surface from the knowledge base - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 2 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'on' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'obj' - arg_msg.value = self.obj - request.knowledge.values.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'plane' - arg_msg.value = self.obj_plane - request.knowledge.values.append(arg_msg) - - self.knowledge_update_client(request) - - # we remove the fact that the robot's gripper is empty from the knowledge base - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 2 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'empty_gripper' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - request.knowledge.values.append(arg_msg) - - self.knowledge_update_client(request) - - # we add the fact that the robot is holding the object to the knowledge base - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 0 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'holding' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - request.knowledge.values.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'obj' - arg_msg.value = self.obj - request.knowledge.values.append(arg_msg) - - self.knowledge_update_client(request) + '''Updates the knowledge base with the following facts: + * the robot is holding the grasped object + * the object is not on the plane anymore + * the robot's gripper is not empty anymore + ''' + facts_to_add = [('holding', [('bot', self.robot_name), ('obj', self.obj)])] + facts_to_remove = [('on', [('obj', self.obj), ('plane', self.obj_plane)]), + ('empty_gripper', [('bot', self.robot_name)])] + self.kb_interface.update_kb(facts_to_add, facts_to_remove) if __name__ == '__main__': rospy.init_node('pickup_client') diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/src/mdr_pickup_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/src/mdr_pickup_action/action_states.py index 195183dc9..efa488a5a 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/src/mdr_pickup_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/src/mdr_pickup_action/action_states.py @@ -1,49 +1,37 @@ #!/usr/bin/python from importlib import import_module +import numpy as np import rospy -import smach import tf import actionlib from geometry_msgs.msg import PoseStamped +from pyftsm.ftsm import FTSMTransitions +from mas_execution.action_sm_base import ActionSMBase +from mdr_move_forward_action.msg import MoveForwardAction, MoveForwardGoal from mdr_move_base_action.msg import MoveBaseAction, MoveBaseGoal from mdr_move_arm_action.msg import MoveArmAction, MoveArmGoal from mdr_pickup_action.msg import PickupGoal, PickupFeedback, PickupResult -class SetupPickup(smach.State): - def __init__(self): - smach.State.__init__(self, outcomes=['succeeded', 'failed'], - input_keys=['pickup_goal'], - output_keys=['pickup_feedback', 'pickup_result']) - - def execute(self, userdata): - # consider moving the other components of the robot out of the arm's way, though - # this could be dangerous if the robot is for example close to some furniture item - - feedback = PickupFeedback() - feedback.current_state = 'SETUP_PICKUP' - feedback.message = '[SETUP_PICKUP] setting up the arm' - userdata.pickup_feedback = feedback - - return 'succeeded' - -class Pickup(smach.State): +class PickupSM(ActionSMBase): def __init__(self, timeout=120.0, gripper_controller_pkg_name='mdr_gripper_controller', pregrasp_config_name='pregrasp', + pregrasp_top_config_name='pregrasp_top', intermediate_grasp_offset=-1, safe_arm_joint_config='folded', move_arm_server='move_arm_server', move_base_server='move_base_server', + move_forward_server='move_forward_server', base_elbow_offset=-1., + arm_base_offset=-1., grasping_orientation=list(), grasping_dmp='', dmp_tau=1., - number_of_retries=0): - smach.State.__init__(self, input_keys=['pickup_goal'], - output_keys=['pickup_feedback'], - outcomes=['succeeded', 'failed']) + number_of_retries=0, + max_recovery_attempts=1): + super(PickupSM, self).__init__('Pick', [], max_recovery_attempts) self.timeout = timeout gripper_controller_module_name = '{0}.gripper_controller'.format(gripper_controller_pkg_name) @@ -52,11 +40,14 @@ def __init__(self, timeout=120.0, self.gripper = GripperControllerClass() self.pregrasp_config_name = pregrasp_config_name + self.pregrasp_top_config_name = pregrasp_top_config_name self.intermediate_grasp_offset = intermediate_grasp_offset self.safe_arm_joint_config = safe_arm_joint_config self.move_arm_server = move_arm_server self.move_base_server = move_base_server + self.move_forward_server = move_forward_server self.base_elbow_offset = base_elbow_offset + self.arm_base_offset = arm_base_offset self.grasping_orientation = grasping_orientation self.grasping_dmp = grasping_dmp self.dmp_tau = dmp_tau @@ -64,24 +55,41 @@ def __init__(self, timeout=120.0, self.tf_listener = tf.TransformListener() - self.move_arm_client = actionlib.SimpleActionClient(self.move_arm_server, MoveArmAction) - self.move_arm_client.wait_for_server() - - self.move_base_client = actionlib.SimpleActionClient(self.move_base_server, MoveBaseAction) - self.move_base_client.wait_for_server() - - def execute(self, userdata): - feedback = PickupFeedback() - feedback.current_state = 'PICKUP' - feedback.message = '[PICKUP] moving the arm' - userdata.pickup_feedback = feedback - - pose = userdata.pickup_goal.pose + self.move_arm_client = None + self.move_base_client = None + self.move_forward_client = None + + def init(self): + try: + self.move_arm_client = actionlib.SimpleActionClient(self.move_arm_server, MoveArmAction) + rospy.loginfo('[pickup] Waiting for %s server', self.move_arm_server) + self.move_arm_client.wait_for_server() + except: + rospy.logerr('[pickup] %s server does not seem to respond', self.move_arm_server) + + try: + self.move_base_client = actionlib.SimpleActionClient(self.move_base_server, MoveBaseAction) + rospy.loginfo('[pickup] Waiting for %s server', self.move_base_server) + self.move_base_client.wait_for_server() + except: + rospy.logerr('[pickup] %s server does not seem to respond', self.move_base_server) + + try: + self.move_forward_client = actionlib.SimpleActionClient(self.move_forward_server, MoveForwardAction) + rospy.loginfo('[pickup] Waiting for %s server', self.move_forward_server) + self.move_forward_client.wait_for_server() + except: + rospy.logerr('[pickup] %s server does not seem to respond', self.move_forward_server) + + return FTSMTransitions.INITIALISED + + def running(self): + pose = self.goal.pose pose.header.stamp = rospy.Time(0) pose_base_link = self.tf_listener.transformPose('base_link', pose) if self.base_elbow_offset > 0: - self.align_base_with_pose(pose_base_link) + self.__align_base_with_pose(pose_base_link) # the base is now correctly aligned with the pose, so we set the # y position of the goal pose to the elbow offset @@ -97,52 +105,73 @@ def execute(self, userdata): retry_count = 0 while (not grasp_successful) and (retry_count <= self.number_of_retries): if retry_count > 0: - rospy.loginfo('[PICKUP] Retrying grasp') + rospy.loginfo('[pickup] Retrying grasp') - rospy.loginfo('[PICKUP] Opening the gripper...') + rospy.loginfo('[pickup] Opening the gripper...') self.gripper.open() - rospy.loginfo('[PICKUP] Preparing for grasp verification') + rospy.loginfo('[pickup] Preparing for grasp verification') self.gripper.init_grasp_verification() - rospy.loginfo('[PICKUP] Moving to a pregrasp configuration...') - self.move_arm(MoveArmGoal.NAMED_TARGET, self.pregrasp_config_name) - - if self.intermediate_grasp_offset > 0: - rospy.loginfo('[PICKUP] Moving to intermediate grasping pose...') - pose_base_link.pose.position.x -= self.intermediate_grasp_offset - self.move_arm(MoveArmGoal.END_EFFECTOR_POSE, pose_base_link) - - rospy.loginfo('[PICKUP] Grasping...') - if self.intermediate_grasp_offset > 0: - pose_base_link.pose.position.x += self.intermediate_grasp_offset - arm_motion_success = self.move_arm(MoveArmGoal.END_EFFECTOR_POSE, pose_base_link) - if not arm_motion_success: - rospy.logerr('[PICKUP] Arm motion unsuccessful') - return 'failed' + if self.goal.strategy == PickupGoal.SIDEWAYS_GRASP: + rospy.loginfo('[pickup] Preparing sideways graps') + pose_base_link = self.__prepare_sideways_grasp(pose_base_link) + + rospy.loginfo('[pickup] Grasping...') + arm_motion_success = self.__move_arm(MoveArmGoal.END_EFFECTOR_POSE, pose_base_link) + if not arm_motion_success: + rospy.logerr('[pickup] Arm motion unsuccessful') + self.result = self.set_result(False) + return FTSMTransitions.DONE + + rospy.loginfo('[pickup] Arm motion successful') + elif self.goal.strategy == PickupGoal.TOP_GRASP: + rospy.loginfo('[pickup] Preparing top grasp') + pose_base_link, x_align_distance = self.__prepare_top_grasp(pose_base_link) + self.gripper.orient_z(pose_base_link.pose.orientation) + + pose_base_link, _ = self.__prepare_top_grasp(pose_base_link) + rospy.loginfo('[pickup] Grasping...') + arm_motion_success = self.__move_arm(MoveArmGoal.END_EFFECTOR_POSE, pose_base_link) + if not arm_motion_success: + rospy.logerr('[pickup] Arm motion unsuccessful') + self.result = self.set_result(False) + return FTSMTransitions.DONE + + rospy.loginfo('[pickup] Arm motion successful') + else: + rospy.logerr('[pickup] Unknown grasping strategy requested; ignoring request') + self.result = self.set_result(False) + return FTSMTransitions.DONE - rospy.loginfo('[PICKUP] Arm motion successful') - rospy.loginfo('[PICKUP] Closing the gripper') + rospy.loginfo('[pickup] Closing the gripper') self.gripper.close() - rospy.loginfo('[PICKUP] Moving the arm back') - self.move_arm(MoveArmGoal.NAMED_TARGET, self.safe_arm_joint_config) + rospy.loginfo('[pickup] Moving the arm back') + self.__move_arm(MoveArmGoal.NAMED_TARGET, self.safe_arm_joint_config) - rospy.loginfo('[PICKUP] Verifying the grasp...') + if self.goal.strategy == PickupGoal.TOP_GRASP: + rospy.loginfo('[pickup] Moving the base back to the original position') + if abs(x_align_distance) > 0: + self.__move_base_along_x(-x_align_distance) + + rospy.loginfo('[pickup] Verifying the grasp...') grasp_successful = self.gripper.verify_grasp() if grasp_successful: - rospy.loginfo('[PICKUP] Successfully grasped object') + rospy.loginfo('[pickup] Successfully grasped object') else: - rospy.loginfo('[PICKUP] Grasp unsuccessful') + rospy.loginfo('[pickup] Grasp unsuccessful') retry_count += 1 if grasp_successful: - return 'succeeded' + self.result = self.set_result(True) + return FTSMTransitions.DONE - rospy.loginfo('[PICKUP] Grasp could not be performed successfully') - return 'failed' + rospy.loginfo('[pickup] Grasp could not be performed successfully') + self.result = self.set_result(False) + return FTSMTransitions.DONE - def align_base_with_pose(self, pose_base_link): + def __align_base_with_pose(self, pose_base_link): '''Moves the base so that the elbow is aligned with the goal pose. Keyword arguments: @@ -168,7 +197,7 @@ def align_base_with_pose(self, pose_base_link): self.move_base_client.wait_for_result() self.move_base_client.get_result() - def move_arm(self, goal_type, goal): + def __move_arm(self, goal_type, goal): '''Sends a request to the 'move_arm' action server and waits for the results of the action execution. @@ -191,15 +220,40 @@ def move_arm(self, goal_type, goal): result = self.move_arm_client.get_result() return result -class SetActionLibResult(smach.State): - def __init__(self, result): - smach.State.__init__(self, outcomes=['succeeded'], - input_keys=['pickup_goal'], - output_keys=['pickup_feedback', 'pickup_result']) - self.result = result - - def execute(self, userdata): + def __prepare_sideways_grasp(self, pose_base_link): + rospy.loginfo('[PICKUP] Moving to a pregrasp configuration...') + self.__move_arm(MoveArmGoal.NAMED_TARGET, self.pregrasp_config_name) + + if self.intermediate_grasp_offset > 0: + rospy.loginfo('[PICKUP] Moving to intermediate grasping pose...') + pose_base_link.pose.position.x -= self.intermediate_grasp_offset + self.__move_arm(MoveArmGoal.END_EFFECTOR_POSE, pose_base_link) + + if self.intermediate_grasp_offset > 0: + pose_base_link.pose.position.x += self.intermediate_grasp_offset + return pose_base_link + + def __prepare_top_grasp(self, pose_base_link): + rospy.loginfo('[PICKUP] Moving to a pregrasp configuration...') + self.__move_arm(MoveArmGoal.NAMED_TARGET, self.pregrasp_top_config_name) + x_align_distance = 0 + if self.arm_base_offset > 0: + x_align_distance = pose_base_link.pose.position.x - self.arm_base_offset + self.__move_base_along_x(x_align_distance) + pose_base_link.pose.position.x = self.arm_base_offset + return pose_base_link, x_align_distance + + def __move_base_along_x(self, distance_to_move): + movement_speed = np.sign(distance_to_move) * 0.1 # m/s + movement_duration = distance_to_move / movement_speed + move_forward_goal = MoveForwardGoal() + move_forward_goal.movement_duration = movement_duration + move_forward_goal.speed = movement_speed + self.move_forward_client.send_goal(move_forward_goal) + self.move_forward_client.wait_for_result() + self.move_forward_client.get_result() + + def set_result(self, success): result = PickupResult() - result.success = self.result - userdata.pickup_result = result - return 'succeeded' + result.success = success + return result diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/CMakeLists.txt b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/CMakeLists.txt index 4e43300ae..d685fbe90 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/CMakeLists.txt +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/CMakeLists.txt @@ -16,7 +16,7 @@ find_package(catkin REQUIRED COMPONENTS rosplan_dispatch_msgs rosplan_knowledge_msgs diagnostic_msgs - mcr_perception_msgs + mas_perception_msgs mdr_rosplan_interface mdr_move_arm_action mdr_move_base_action @@ -48,7 +48,7 @@ catkin_package( geometry_msgs trajectory_msgs diagnostic_msgs - mcr_perception_msgs + mas_perception_msgs mdr_rosplan_interface mdr_move_arm_action mdr_move_base_action diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/README.md b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/README.md index 3e0c135e2..444df01db 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/README.md +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/README.md @@ -71,8 +71,6 @@ The following parameters need to be passed when launching the action client: * ``action_timeout``: Maximum time (in seconds) that we are willing to wait for the action to be executed * ``action_dispatch_topic``: Name of the topic at which the plan dispatcher sends action requests * ``action_feedback_topic``: Name of the topic at which the action sends feedback to the plan dispatcher -* ``knowledge_update_service``: Name of a service used for updating the planning problem as the world changes -* ``attribute_fetching_service``: Name of a service used for retrieving attributes representing the current knowledge about the world * ``placing_pose_frame``: Name of the frame in which placing is performed (default: 'base_link') ## Action execution summary @@ -94,7 +92,7 @@ The action performs placing with respect to the `base_link` frame (even if the g * ``rosplan_dispatch_msgs`` * ``rosplan_knowledge_msgs`` * ``diagnostic_msgs`` -* ``mcr_perception_msgs`` +* ``mas_perception_msgs`` * ``mdr_rosplan_interface`` * ``mdr_move_arm_action`` * ``mdr_move_base_action`` diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/config/trajectory_weights/weights_table_place.yaml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/config/trajectory_weights/weights_table_place.yaml index 6a97b7cbb..2bcb6488b 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/config/trajectory_weights/weights_table_place.yaml +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/config/trajectory_weights/weights_table_place.yaml @@ -1,207 +1,1667 @@ -pitch: [-35.42493997049422, -37.70819524763274, -41.30898221809194, -46.91255259713179, - -54.67543853474187, -63.46993205757798, -70.71710907291556, -73.40838683036453, - -69.88965835948521, -61.23837968207681, -51.46297199294242, -46.651105552304536, - -53.51199582034473, -77.36775616098751, -119.40733708986616, -173.73781284783232, - -226.1456109740379, -256.9255230366232, -248.19097106023983, -192.5413353804086, - -97.86144890114835, 15.403191745759875, 123.77904323809733, 211.94253960392064, - 278.4370219521786, 331.32494006867165, 376.0838394153919, 404.99584445075004, 397.5504226626487, - 333.15522844035416, 207.40468267623098, 40.23505964106349, -129.5637520862881, -259.90824743283804, - -322.2438715398654, -311.38242615291654, -244.58025251479793, -151.69677985653905, - -62.27097378404166, 4.242548228062668, 41.72701342077948, 54.61937386645844, 52.0678236639199, - 42.46420503183177, 30.605603045097798, 17.831640535863865, 3.9115768409267537, -11.035226656357036, - -25.73694492619038, -38.23525698681982, -46.96747089197185, -51.55412317003241, - -52.72523403265943, -51.52230673716135, -48.47287389295395, -43.418549291861005, - -36.12710203132058, -27.217737390364878, -18.73250127108376, -13.940788921463144, - -16.383140380541224, -28.479814446323395, -50.19840934679473, -78.34588385548898, - -106.9796715308115, -129.0771227243504, -138.96143934459482, -134.39916286752, -117.2699501961774, - -92.4438455558197, -65.54260990887046, -40.863874703900365, -20.503006515797157, - -4.857673288780039, 6.139265967008193, 12.034840538128108, 11.78201008858928, 4.554178806755324, - -9.17603539972846, -27.06452970168944, -45.243054184153266, -59.42498559615973, - -66.20027581755308, -63.910105987553706, -52.76476291531106, -34.26469807990957, - -10.31714588108525, 17.428528508170896, 47.650311361280316, 78.83964513599082, 108.6479511556088, - 133.71712237097847, 150.35674122169067, 155.84935265330853, 149.667219171301, 133.86087612595915, - 112.3745444005185, 89.69640055960969, 69.59508619729118, 54.52489160570828, 45.78344309013908, - 44.04929137036943, 49.782033634194036, 63.15855610291111, 83.57913311970796, 109.088551720991, - 136.1606322569329, 160.14074686489496, 176.30498395284482, 181.14929667909337, 173.36150369292395, - 154.05941286281936, 126.24020279597528, 93.77525021113942, 60.472164188134116, 29.603993012435218, - 3.958702688619639, -13.885207810920182, -21.476471710992165, -16.848997943544642, - 0.7832249798221619, 30.385533864461852, 68.93627511091994, 111.90269031244195, 154.16251910598547, - 190.93981622752082, 218.36838869956537, 233.58094610071046, 234.54607891687763, - 220.01454742143613, 189.79459560980567, 145.2497513417973, 89.62834985791211, 27.797829306827328, - -34.782880289602524, -93.6044248049321, -146.58335798475517, -194.84023705533775, - -242.3963649186648, -294.83526437988365, -357.38490486635334, -433.09803303759264, - -521.7578411720901, -619.8377564762594, -721.4265148901123, -819.6866688138135, - -908.3015435028083, -982.4985157361981, -1039.4942245798738, -1078.4361122467722] -roll: [-94.9809024798238, -89.37326678075686, -74.67496646210842, -49.97877940131072, - -17.593122681262724, 16.06645308336598, 41.71318415962005, 50.60713287019311, 38.51880205462394, - 8.097319900299366, -31.598013501826173, -68.24707622586226, -90.81627135482617, - -93.60576994022517, -77.98514286453168, -51.10227044659737, -22.50696815786439, - -0.5347663027544345, 10.124158386418726, 9.150782393854067, -0.3095242652998163, - -13.68185030980892, -27.163152096763604, -39.030814562375696, -49.5151324263574, - -59.485126018140726, -68.80624552086546, -75.428898637592, -75.92665061062036, -67.34610061221686, - -49.32728657858986, -25.192438385029675, -1.2918864998652975, 15.141124521025375, - 18.667930530166085, 7.7185754208041795, -14.77455802286933, -42.4332819435094, -67.77813685589356, - -84.93421974950635, -91.54423345826856, -89.02137901209117, -81.13057995433573, - -71.8508278902564, -63.789491345075696, -57.835970205818235, -53.8231627143478, - -51.463590908332876, -50.94238773923665, -52.942655659661675, -58.189136134695445, - -66.74155669337485, -77.33961889552769, -87.14688594433058, -92.1973263131889, -88.62131828366893, - -74.32902174902947, -50.44322013852119, -21.67409515681044, 4.817148616819843, 21.806399412879404, - 24.68253720151413, 13.196955457999769, -8.512322146430131, -33.57118762493018, -54.88516872102151, - -67.45252093691383, -69.67085932958665, -63.35440572936342, -52.67299311155918, - -42.53041326451286, -36.97101954194654, -38.07581556440504, -45.58076611309898, - -57.22308967053014, -69.64023776501026, -79.50688077198373, -84.51353160768544, - -83.83257156823157, -77.93238694773984, -67.92280694397907, -54.86274891199053, - -39.45901730010299, -22.31036765250445, -4.4655218706363, 12.1844480635262, 25.094101784610512, - 31.88594109540416, 31.426514019400226, 24.561406129229585, 14.071733616471022, 3.8310309701905063, - -2.4667652078742877, -2.4910925874960017, 4.036658820887705, 15.329450275859665, - 28.159760716196264, 38.92003600168367, 44.79013991534497, 44.63012010630281, 39.28967211649084, - 31.232881080163466, 23.636257019694135, 19.304811972720493, 19.785502677120114, - 24.950320982296397, 33.14353632090505, 41.81125510618662, 48.398727074838696, 51.231113704687615, - 50.09924343700417, 46.35777463863367, 42.492653842074425, 41.28546877990567, 44.839129747983, - 53.785840236969825, 66.95276774389771, 81.62171724086008, 94.3240624864676, 101.92183810445113, - 102.60933075138487, 96.47920077162367, 85.44154179319236, 72.51515576452759, 60.73862809903186, - 52.084774877874715, 46.75751956690583, 43.11719157604091, 38.275825894925006, 29.196437849286824, - 13.971039246031658, -7.127902794056211, -31.207065658218177, -53.168507134733815, - -66.97618274894776, -67.45714153993094, -52.067059591180616, -21.977834522009797, - 17.97026793057221, 60.50618410267125, 97.94258360415643, 124.23605347540033, 136.35854432363232, - 134.57499001187503, 121.7126248618848, 101.89089552853724, 79.27247943521832, 57.21560075628847, - 37.928073989631585, 22.51410879818383] -x: [-23.54082186527676, -23.512905258800767, -23.516224768121276, -23.509127341780307, - -23.454121068234553, -23.37527958550504, -23.398062947937724, -23.714937560177475, - -24.478124554177448, -25.68503685748029, -27.12817117252123, -28.444857414576866, - -29.255164972260452, -29.32533200992744, -28.663538268050935, -27.485379784503554, - -26.068328960660104, -24.58356824095513, -23.00962769946186, -21.200239596466837, - -19.096858163949097, -16.95368487091472, -15.374423042071834, -15.062830081970883, - -16.420337211023146, -19.266187917922952, -22.863442272556544, -26.212763756336138, - -28.43414844400656, -29.064052770088523, -28.17268699560032, -26.28311891920491, - -24.142369984008027, -22.446184009232976, -21.626313157732994, -21.768723364314585, - -22.670561864739494, -23.99022574778685, -25.41383154390955, -26.762121364597242, - -27.994402597101168, -29.11804796936013, -30.06316239908554, -30.607870142419564, - -30.41937426536905, -29.21126868808186, -26.94246526112341, -23.943122787757076, - -20.87495709704674, -18.512790864664698, -17.435764167233458, -17.78028043031174, - -19.180496230067334, -20.915425140594678, -22.174498281441092, -22.3240925499991, - -21.103563664135123, -18.729923248158855, -15.895248710835736, -13.624755694929485, - -12.985496396331671, -14.71180186829232, -18.89175793950522, -24.869265613923826, - -31.43499308439889, -37.24493024420751, -41.28732213685973, -43.18410476572519, - -43.194339294574945, -41.951140068617846, -40.10893022819285, -38.10222626963581, - -36.10877330115395, -34.16247721797567, -32.287238408773284, -30.55543065783834, - -29.06037228818418, -27.855098125159977, -26.91524249105149, -26.147620843657243, - -25.42616007720281, -24.62192925623665, -23.60773081960084, -22.241953853405278, - -20.350308251339122, -17.723112367242447, -14.141853873202399, -9.449513334682687, - -3.6720025070654474, 2.8362755406261857, 9.303036300724449, 14.609576588230889, - 17.5737981092729, 17.371008465565403, 13.903729103835676, 7.920315318993102, 0.7993036840940881, - -5.90970951170443, -10.979658455755517, -13.829074454257093, -14.5755935725983, - -13.85183635092414, -12.500079190061692, -11.274722889530162, -10.641362881668742, - -10.704587515925214, -11.250922772773135, -11.867715076437035, -12.089807887886105, - -11.528372274762951, -9.948640513140584, -7.285309022869411, -3.611258670618167, - 0.9044519621289198, 6.012648194756425, 11.361212296824625, 16.45810776843166, 20.673183290896244, - 23.327284211933843, 23.874543031911173, 22.122886022700243, 18.39546579340212, 13.54233035939333, - 8.770094314293303, 5.339138669234294, 4.239493164175406, 5.965509978348073, 10.462318872054993, - 17.240025852914957, 25.58456988711478, 34.768784271394836, 44.19101560731903, 53.42265111802887, - 62.1967348811316, 70.38947366683362, 78.02811866838204, 85.31956397263224, 92.66238947803912, - 100.60176750352969, 109.7128630951549, 120.43768024118057, 132.93038931028732, 146.9707364427417, - 161.98099303697487, 177.14055334845793, 191.554949364158, 204.4214845608734, 215.14598259878588, - 223.39255572340488, 229.07401275593168] -y: [-48.08181651846675, -49.587117112576244, -50.9623587336631, -52.74638540311553, - -55.339316884123335, -58.187634767212785, -59.19858890679832, -55.18823159102758, - -43.45669279583084, -23.680143811777636, 1.0278742629116866, 24.803094657233476, - 41.56642405836774, 48.12670154484163, 45.66964260010242, 38.41392701146586, 30.559274820896626, - 24.01469229055469, 18.37242623227647, 12.461922135369557, 5.780911723761772, -1.2306825260415035, - -7.855647675106398, -13.850897756070554, -19.628125426393403, -25.766439690878475, - -32.25785903733505, -38.106735397658724, -41.700722135824165, -41.78689726002638, - -38.35450818167012, -32.77088479939355, -27.13457755859672, -23.383886152709735, - -22.698519255950604, -25.30103433852694, -30.459665680447525, -36.587875594965475, - -41.56956109814254, -43.41643845983297, -41.04609408947397, -34.71380717009749, - -25.770834909882954, -15.877657193068476, -6.18267876618329, 3.0042222833118606, - 11.840355394622723, 20.142092255935047, 26.74289840637122, 29.592807279096725, 26.707565820644888, - 17.456235617422454, 3.388730612913831, -11.925225381003171, -23.997617973752774, - -28.962264558977488, -24.922343000496628, -12.598312313006955, 4.970869040522485, - 23.575824375424354, 39.379440684218075, 50.19400929294163, 55.947550262636035, 58.225838746081386, - 59.27299512331673, 61.046406924836795, 64.7113242754852, 70.55113102435496, 78.04412796628496, - 85.97088366315737, 92.64894651093383, 96.42012688093448, 96.28287075303587, 92.33099685071431, - 85.70473506502236, 78.06766687875744, 70.92429970225633, 65.1466098402518, 60.88285588515454, - 57.769140305510135, 55.238497697808114, 52.76635428971193, 50.01647387605132, 46.93679691285544, - 43.83967985214271, 41.42191756324371, 40.63190289145288, 42.344396617716775, 46.9347863760049, - 53.95190906255407, 62.07786213730404, 69.43105232791753, 74.09941024378266, 74.68381768815975, - 70.63554962109797, 62.27282910008037, 50.51258663591012, 36.491359876837045, 21.304370351168693, - 6.013296228474457, -8.122423557377378, -19.438718195250082, -25.98893089734145, - -26.128853719153366, -19.38244812067696, -7.151684031278839, 7.173184693921752, - 18.886623201163076, 23.235809508225923, 16.81931017033493, -1.4029158802374324, - -29.8433104623529, -64.82075623228921, -101.63386062733136, -135.70233578562002, - -163.3136997983213, -181.76783190637622, -189.09357664198424, -183.78138421191431, - -164.93275730603818, -132.8760319352321, -89.87628487198717, -40.3823840377136, - 9.545608199456396, 53.52353674119033, 86.31999893025124, 104.89002936158305, 108.59148361413862, - 98.65485183597463, 77.326293537273, 47.16357178564959, 10.74497388276303, -29.265103349701626, - -69.94908118242546, -108.10390173253013, -140.4074927252729, -163.68216316743766, - -175.12064015744392, -172.44337814636657, -154.07896535435387, -119.47006083653432, - -69.47218086412778, -6.639348762292799, 64.85632773989234, 139.77404187801, 212.67787306545785, - 278.82692966691434, 334.7974511897336, 378.7116450336994, 410.118700189302] -yaw: [-53.959788987781124, -53.30371682466102, -50.30427376032306, -45.009643854796366, - -38.23858486542749, -31.445920900106113, -26.17708607783723, -23.44624165703102, - -23.41933783360354, -25.515961378948077, -28.81688930020335, -32.56774753973497, - -36.529787446838284, -40.964730323552274, -46.18201275969121, -51.81029740608197, - -56.21012654557379, -56.53909770605108, -49.73151173357274, -34.11186523750416, - -10.827794825167219, 15.891304347107724, 40.20849702734764, 57.368805169815346, - 65.95177865876515, 67.97159561584449, 66.64040895830699, 63.589047697786704, 57.82287342197275, - 47.11725574000715, 30.46733711609649, 9.533355760921287, -11.897470382363254, -29.830521402437533, - -41.96018651062839, -48.39143034359017, -50.90765467089368, -51.442211403866196, - -50.88101688407953, -48.972314263916196, -45.17504471685727, -39.55329712338891, - -32.95725845941995, -26.49322988683765, -20.878353476637844, -16.223447732915336, - -12.318730267752962, -9.114208916103852, -7.025318075714705, -6.863715409937331, - -9.418953881919258, -14.892711365810932, -22.46579817424915, -30.24344467810545, - -35.69109978319271, -36.477876243338805, -31.444025538076012, -21.288670881670154, - -8.62375068107963, 2.723377241865635, 8.950421398026423, 7.60130696452477, -1.5512568272241891, - -16.415907780851523, -33.32800892857806, -48.34061652494549, -58.544149663334146, - -62.8778987289918, -62.129401745774736, -58.221245342094605, -53.23124870684945, - -48.65125161003768, -45.139206829597285, -42.67849855974542, -40.883183302808774, - -39.24556037081911, -37.279870735621394, -34.610218273166865, -31.036388602506094, - -26.556600636656302, -21.314340237646586, -15.485545852961351, -9.182367311554216, - -2.4539357174518477, 4.608108661974362, 11.748942419186927, 18.53564938062657, 24.488775175064866, - 29.268641679431983, 32.76438623868329, 35.01433031937393, 36.04103332494177, 35.76699835677284, - 34.10630639596401, 31.16848412968319, 27.407761733176166, 23.587197726368412, 20.558872791588364, - 18.97592238631098, 19.07565937416213, 20.61709103744411, 22.979740291136025, 25.377603643755922, - 27.117925844469166, 27.82778995770433, 27.579205757414403, 26.87117155011749, 26.47346470212829, - 27.18502510687263, 29.588305367087866, 33.87866838785326, 39.818047979613915, 46.81614514744827, - 54.094984627640684, 60.861552883388576, 66.41686398223497, 70.17429953524406, 71.62539144890958, - 70.3348055057656, 66.03071453327549, 58.78227299675599, 49.168265349183585, 38.3031310449789, - 27.633806894374317, 18.533974132396466, 11.83869780529428, 7.516135260286569, 4.634765508402249, - 1.6752451183326715, -2.8928198989370846, -9.947988875516257, -19.190646683273787, - -28.95981792942289, -36.52500660659488, -38.85392526474802, -33.68717119866384, - -20.59129682191029, -1.5875034968128299, 18.968950912034025, 35.341880979175166, - 41.95032447278416, 34.96068739312236, 13.34321915944511, -20.982377451382774, -63.886780871889336, - -110.27067151277551, -155.29388790639618, -195.21850221882582, -227.74597004288862, - -251.94049712978926] -z: [-26.674573624633936, -27.114809002109638, -27.80093370552566, -28.836873185567413, - -30.21881883969757, -31.695951164601357, -32.72354364740953, -32.62183352655716, - -30.90258581567612, -27.583495683677555, -23.297201486579876, -19.087260962922908, - -15.936418055004204, -14.273620356202283, -13.80346926204938, -13.802500150724061, - -13.630619842081579, -13.020757497121616, -11.96881998725885, -10.469674381861992, - -8.452160823356074, -5.952178992153526, -3.225706585502589, -0.5697494663204837, - 1.9808009060413083, 4.68376066797161, 7.7212422921196655, 10.792547098296602, 13.147282962472149, - 14.103314291770715, 13.571455393667573, 12.085886522451826, 10.339296245986802, - 8.68602893983164, 7.033777346134439, 5.120448773462501, 2.8513690211156364, 0.4137469421774722, - -1.8591108055200387, -3.7053995669548203, -5.044430532720517, -5.89457353611374, - -6.160820814559367, -5.4596622601026645, -3.15564680899608, 1.3247372084788658, - 8.009198179393971, 15.980184925805682, 23.347897126275733, 27.749074489647064, 27.248379889872915, - 21.249227180863592, 10.952075671055974, -0.9012226404090096, -10.918446805389463, - -16.22596783788798, -15.453611074433008, -9.153655798428526, 0.49699749838585544, - 10.561454823123338, 18.391516558218946, 22.436392554843273, 22.545534532960506, - 19.747590276327738, 15.689361506049153, 11.99803586193934, 9.79057402569709, 9.448977960828584, - 10.667279692473349, 12.692684350192357, 14.645560060408101, 15.811942817737004, - 15.84203100562251, 14.829843387399771, 13.271968183018519, 11.913283989441497, 11.507542003036887, - 12.558104629802221, 15.134221452069113, 18.84592814794398, 22.998757891465335, 26.871030400509593, - 30.007884926015393, 32.42946843702773, 34.691007713089675, 37.77704102447371, 42.83936321417833, - 50.80550353800271, 61.91981744856823, 75.34762579946161, 89.04033240312027, 100.0468270773533, - 105.30303255039908, 102.67548082168979, 91.82574965851784, 74.46578861532257, 53.83412456102248, - 33.60637700701678, 16.731362600762733, 4.69041935031565, -2.5784558424742117, -6.19851466771825, - -7.652486552004426, -8.203701204838094, -8.60951161090784, -9.098233442799044, -9.48718196283113, - -9.346536633833383, -8.18047015987454, -5.61948916274534, -1.5868054321866703, 3.6274373814820575, - 9.426783721975054, 15.078581176949248, 19.909144614716634, 23.43487153892179, 25.38970214772102, - 25.6741236265473, 24.288580218363613, 21.306889896281145, 16.904641282521364, 11.411558969184892, - 5.333027801777911, -0.7017358032446038, -6.065575734065763, -10.287538553248018, - -13.167739669212384, -14.80357049337103, -15.524777375079806, -15.780272101184652, - -16.035519447260974, -16.72035793136546, -18.231003979213103, -20.961756875344914, - -25.33507117144374, -31.809284959656072, -40.85818085498114, -52.925248528929615, - -68.35651409024713, -87.31513845671434, -109.68639958664852, -134.99539426547764, - -162.3735258302758, -190.60811846102203, -218.28355820802886, -243.98292875048506, - -266.49124025702065, -284.9430654469704, -298.88475826361605, -308.25498187704557] +pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766, + -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698, + -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024, + -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475, + -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925, + -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358, + -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696, + -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558, + -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959, + -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712, + -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775, + -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227, + -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924, + -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524, + -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154, + -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213, + -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968, + -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885, + -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287, + -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954, + -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699, + -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334, + -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104, + -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568, + -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354, + -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645, + -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636, + -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587, + -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638, + -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972, + -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447, + -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077, + -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827, + -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278, + -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243, + -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458, + -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384, + -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971, + -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393, + -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256, + -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992, + -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318, + -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932, + -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875, + -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126, + -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044, + -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763, + -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265, + -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126, + -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113, + -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113, + -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535, + -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696, + -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247, + -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216, + -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353, + -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494, + -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333, + -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105, + -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883, + -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927, + -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884, + -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401, + -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479, + -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717, + -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403, + -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617, + -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946, + -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314, + -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325, + -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804, + -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694, + -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923, + -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567, + -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941, + -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014, + -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199, + -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829, + -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106, + -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166, + -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521, + -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426, + -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046, + -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633, + -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434, + -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539, + -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053, + -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804, + -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645, + -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914, + -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364, + -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178, + -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894, + -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255, + -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792, + -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684, + -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029, + -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007, + -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605, + -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889, + -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275, + -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017, + -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505, + -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556, + -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376, + -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741, + -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233, + -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081, + -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436, + -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614, + -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242, + -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417, + -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184, + -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262, + -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124, + -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636, + -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339, + -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535, + -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313, + -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217, + -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734, + -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575, + -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034, + -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034, + -420.89393277454684] +roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766, + -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698, + -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024, + -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475, + -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925, + -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358, + -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696, + -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558, + -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959, + -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712, + -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775, + -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227, + -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924, + -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524, + -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154, + -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213, + -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968, + -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885, + -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287, + -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954, + -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699, + -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334, + -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104, + -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568, + -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354, + -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645, + -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636, + -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587, + -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638, + -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972, + -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447, + -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077, + -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827, + -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278, + -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243, + -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458, + -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384, + -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971, + -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393, + -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256, + -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992, + -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318, + -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932, + -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875, + -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126, + -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044, + -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763, + -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265, + -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126, + -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113, + -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113, + -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535, + -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696, + -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247, + -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216, + -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353, + -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494, + -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333, + -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105, + -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883, + -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927, + -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884, + -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401, + -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479, + -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717, + -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403, + -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617, + -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946, + -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314, + -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325, + -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804, + -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694, + -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923, + -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567, + -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941, + -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014, + -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199, + -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829, + -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106, + -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166, + -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521, + -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426, + -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046, + -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633, + -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434, + -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539, + -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053, + -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804, + -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645, + -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914, + -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364, + -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178, + -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894, + -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255, + -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792, + -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684, + -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029, + -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007, + -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605, + -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889, + -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275, + -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017, + -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505, + -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556, + -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376, + -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741, + -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233, + -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081, + -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436, + -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614, + -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242, + -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417, + -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184, + -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262, + -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124, + -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636, + -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339, + -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535, + -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313, + -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217, + -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734, + -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575, + -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034, + -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034, + -420.89393277454684] +x: [-131.00261478679988, -121.88886417422353, -111.9050389833139, -102.61368963497223, + -96.05947080078046, -94.12535169815399, -97.8607435820271, -107.14613364957096, + -120.82659085972966, -137.19720078410953, -154.5163549178483, -171.18694825080595, + -185.5568218223094, -195.704691059387, -199.6329868378576, -195.91936207338776, + -184.48915098542318, -167.07793507920195, -147.0574840422674, -128.53128958566316, + -115.04358889413827, -108.56400855374055, -109.19561099927448, -115.57413755530115, + -125.64926844955686, -137.46655539270932, -149.61816709281686, -161.26501144566066, + -171.91636446335687, -181.2216960780747, -188.87007846616464, -194.53289513944736, + -197.79145167358277, -198.0996571089822, -194.8802396222356, -187.78401698176944, + -177.02826515891735, -163.64756841648978, -149.46450567572836, -136.67890898964777, + -127.21395723237792, -122.15440349976664, -121.54367242044958, -124.56838026444774, + -129.9847428341204, -136.57776680869415, -143.4619977851926, -150.1463172881614, + -156.43047062938794, -162.25579819785062, -167.580799896415, -172.2857363783464, + -176.10313080152142, -178.60602094331426, -179.29466139771722, -177.77366590562514, + -173.94835807935246, -168.14763364573156, -161.10576879062413, -153.79437054295704, + -147.18005615509452, -142.03544698559352, -138.8812834694729, -138.0254327793118, + -139.60154077046317, -143.53795283987066, -149.46190564030064, -156.61307764750555, + -163.87292592523232, -169.97827533844975, -173.87402597845175, -175.0512378675456, + -173.7109894894428, -170.68112972530366, -167.11714893622806, -164.10386427943158, + -162.31622495964814, -161.86114761154576, -162.33151047253835, -163.02192881838113, + -163.2095501914243, -162.39248606696069, -160.40951035619491, -157.42206476020834, + -153.79159590063577, -149.91799716532543, -146.1123968434442, -142.55104990302257, + -139.31149673683075, -136.46131741930625, -134.16539879470983, -132.7805579230332, + -132.8976720404706, -135.27842864395015, -140.64645739243105, -149.3508159834204, + -161.00381971456645, -174.2605798886825, -186.9134811438676, -196.38261868432437, + -200.49211090980378, -198.23763830134754, -190.21480773205266, -178.51115567595826, + -166.0801280692791, -155.83164235826004, -149.80028431814193, -148.6986554765198, + -151.96202945833986, -158.18027510739103, -165.68937034330935, -173.07927043758525, + -179.46192687105753, -184.482441275634, -188.1614046703995, -190.6833573748501, + -192.21809683888495, -192.81960540582097, -192.41403431825069, -190.86577715074623, + -188.09301880254696, -184.18878513004086, -179.4955981906823, -174.5892113462465, + -170.1561255168205, -166.79776163996192, -164.8373602238494, -164.2126907946925, + -164.50294621069565, -165.0839384166419, -165.3557930296701, -164.95884643563477, + -163.898412256587, -162.534820649653, -161.44472376807636, -161.20582442689792, + -162.18450627688458, -164.40124437891336, -167.51392693816024, -170.91206169710102, + -173.8749652264376, -175.73083709030058, -175.97063351490164, -174.31004528380663, + -170.72511032331406, -165.48897950595216, -159.20946482285498, -152.82808750163824, + -147.5191667738631, -144.44886967752777, -144.4240273588793, -147.54449659368146, + -153.01617480666846, -159.25176211818498, -164.2877168274527, -166.41140144219656, + -164.78014816559207, -159.78674905727087, -153.00534915065177, -146.70549554069234, + -143.09178186665255, -143.5446658872191, -148.14637102277132, -155.6663204124734, + -164.00416538606794, -170.916565992789, -174.75161613413627, -174.9259359923934, + -171.99663265443107, -167.34336788125873, -162.6147423868518, -159.16048671989986, + -157.6477458947188, -157.96425018036857, -159.39300876091175, -160.95217491299363, + -161.7572333232152, -161.28442609094157, -159.4763125555903, -156.69932273396668, + -153.6090737832136, -150.9888364227075, -149.60413892450302, -150.0809012374839, + -152.79153608790728, -157.7397093479904, -164.46467355593433, -172.0172934256874, + -179.0661158741304, -184.15783677719142, -186.08855140295393, -184.27238379689192, + -178.96525805487465, -171.2372865037682, -162.67650112956372, -154.91277319136154, + -149.12741792008617, -145.72381647659327, -144.27134285886908, -143.72945580899287, + -142.85723879551645, -140.65447875831518, -136.68276519662683, -131.17312406130938, + -124.91209567953047, -118.9751543144055, -114.4168612960534, -112.01955243552163, + -112.15689719095518, -114.77332263747567, -119.44360198084003, -125.471236872446, + -132.00253791160844, -138.1561515203214, -143.17484043177828, -146.58907912429945, + -148.3511438380617, -148.87742279194816, -148.94591893398302, -149.43982973654823, + -150.99302147422435, -153.65215284576814, -156.69307614107936, -158.6972973355033, + -157.91096745292066, -152.8004175957856, -142.62726270976202, -127.83568739093043, + -110.09574251961111, -91.96442744166842, -76.26517339306562, -65.38756088681923, + -60.727455422828086, -62.41769838627875, -69.38103902919622, -79.63028303419432, + -90.6904492501862, -100.0330543381811, -105.46650890222921, -105.47370228165889, + -99.49440285791094, -88.11732926351014, -73.10854974665124, -57.20048636703596, + -43.619919989560685, -35.42935377805215, -34.84789242860901, -42.75227348465364, + -58.50946410168745, -80.17821602052324, -104.99065766337972, -129.94273353227604, + -152.31614487459336, -170.01709776381944, -181.70800379508364, -186.78019457154642, + -185.24347046638363, -177.59665180543396, -164.71393092910603, -147.75511927137686, + -128.09259386421706, -107.2415258381779, -86.77724029789665, -68.22375027449615, + -52.90610923413581, -41.779927174503385, -35.27752870209294, -33.22579478747385, + -34.88146090859914, -39.094144062824924, -44.55933540558859, -50.084953618202576, + -54.78496998250598, -58.13877584276208, -59.90622409641475, -59.945540069741, -58.02266713890794, + -53.710211558038985, -46.44646056518766, -35.76628477961155, -21.64346099537084, + -4.823099943406338, 13.00074336490228, 29.276996809341675, 40.992025758372115, 45.29133769691425, + 40.16014663983825, 24.958013623469462, 0.6430403690297408, -30.37648983818459, -64.70364605521273, + -98.5780766753714, -128.49010205980096, -151.64916889145232, -166.24865624155242, + -171.54102444522545, -167.77666186080413, -156.06265128020192, -138.17902607586348, + -116.36917642692488, -93.10932192371236, -70.85982959536072, -51.803566443571675, + -37.58102962803787, -29.04191085373232, -26.05140387409622, -27.411942444947794, + -30.971298702656785, -33.967139701487234, -33.59780771242022, -27.721946232536823, + -15.50944865087424, 2.165242029053751, 22.75109265450623, 42.51462881068035, 57.39178500711525, + 63.9603626762866, 60.24680348570627, 46.13482203280333, 23.280449691574223, -5.396531697017779, + -36.44546991778746, -66.59572190185905, -93.244393312115, -114.63002544381644, -129.7277803714285, + -137.99626292368302, -139.13919014515983, -133.01230035499455, -119.72497935278233, + -99.88864280213238, -74.88535670756845, -46.99987777401981, -19.290622507018494, + 4.836900082983619, 22.275753850864515, 30.9051736945987, 30.102091149122813, 20.89767984126534, + 5.709426552642763, -12.280539670085691, -29.912602539696113, -44.76409817954616, + -55.587777836517255, -62.37400184312159, -66.06509298635545, -68.05618356119531, + -69.6650415443935, -71.73438308006683, -74.46044693767506, -77.45330286152975, -79.96093287153582, + -81.15272296993173, -80.36351741010003, -77.23600916293604, -71.74756168638527, + -64.14841195780141, -54.85947253693819, -44.377179252428895, -33.21572713174938, + -21.893453397774184, -10.949679527408357, -0.9673128756345823, 7.423219742995925, + 13.573678927669173, 16.882316200266693, 16.880518451590333, 13.321463895553073, + 6.2494544216467425, -3.9662652022608986, -16.64105804227179, -30.827458079441, -45.40427423165941, + -59.19176164715269, -71.07926973763034, -80.1486617898492, -85.77441240031021, -87.6818651318595, + -85.95088814283162, -80.96333944498441, -73.30666637565525, -63.65782924774226, + -52.676828368491805, -40.93506347992221, -28.891766117399765, -16.916080004322772, + -5.3384835550584935, 5.4924058263232025, 15.171162899221802, 23.231051427618734, + 29.178380505050853, 32.559117153019926, 33.042995254450894, 30.506717141912656, + 25.098309221713915, 17.267877434957573, 7.754667766136128, -2.4738852870074073, + -12.328160893560188, -20.73549791063537, -26.79110541102302, -29.892155231726377, + -29.825103487837175, -26.78324113785426, -21.306403690118064, -14.15394397582623, + -6.139545262622449, 2.0335265366227753, 9.89870331065091, 17.286397543623664, 24.307308641748374, + 31.26525601401877, 38.52026696289829, 46.33161129748563, 54.71416332554038, 63.34061174435135, + 71.51679336934421, 78.24704133338132, 82.39018658725793, 82.88576123321361, 79.00811170033668, + 70.59040844420252, 58.15831519340492, 42.928737491281964, 26.660959489767816, 11.387156505595188, + -0.9161323493847441, -8.636955778598946, -10.746100985455664, -6.88302166146115, + 2.6901550272644528, 17.227609362490647, 35.67099777743684, 56.793973316018906, 79.28492204264502, + 101.76799145249652, 122.79490061004348, 140.8566568494178, 154.45830822835748, 162.2730575641024, + 163.35441171879694, 157.35115570417733, 144.6537822686842, 126.41052162787045, 104.38486553091724, + 80.67370587471055, 57.34955312756698, 36.11589971154659, 18.06234868471712, 3.576456180471944, + -7.5771785605298545, -16.047264197592554, -22.625071287068153, -27.99125205987563, + -32.544187050649704, -36.342770320132814, -39.160889188810124, -40.621845381417664, + -40.36566724147911, -38.20206823583871, -34.21329090178294, -28.788359919239177, + -22.587615544563256, -16.449965238654954, -11.263594052219574, -7.82420704968046, + -6.7043106382969215, -8.153633537426817, -12.045315570129013, -17.87558753135798, + -24.817072522162295, -31.818486356751524, -37.73735081273553, -41.48808157884077, + -42.185843095170476, -39.266966337888135, -32.56948414179687, -22.362390213218752, + -9.319317728622229, 5.559271102094557, 21.062973392548795, 35.90860732266084, 48.87612268299202, + 58.92359238196129, 65.26758898268541, 67.42353430763573, 65.20869447959286, 58.716140928984274, + 48.27012084952794, 34.372265068707556, 17.645386644363096, -1.221017147956034, -21.52101235133283, + -42.57613526226664, -63.77089174961588, -84.5781075128901, -104.57331194568057, + -123.43864361815416, -140.9579908724867, -157.005848574077] +y: [-99.19714419940159, -79.16394233266908, -57.87218080909418, -39.28438472983111, + -28.386800564847263, -29.413961436687575, -43.913473953576144, -69.67422837210728, + -101.14133069492664, -131.32148548272554, -154.36897298960562, -167.56000537729983, + -171.8480811942197, -171.05140505400772, -170.15303380273187, -173.34724789149277, + -182.51652550260764, -196.6077612179733, -212.0578803921079, -224.1551729270074, + -228.85174829123696, -224.25993586587964, -211.25206790585622, -193.0143316349947, + -173.74730833399678, -157.0391212367104, -144.63181352345478, -136.04353714588737, + -129.03535351100325, -120.64812612176986, -108.4578159331508, -91.66712058998779, + -71.6845532860305, -51.92454636690193, -36.75664559094005, -29.944229192958293, + -33.26253995101549, -45.8841436370077, -64.72238023555612, -85.55581883977085, -104.43639262659643, + -118.73440682883057, -127.4439469965765, -130.85855117578427, -129.9883150575577, + -126.05068565061471, -120.20491993068747, -113.51752511264922, -107.00751318510588, + -101.6120221067554, -98.037143648059, -96.5901783209796, -97.11117132586354, -99.05464430683851, + -101.69784273360709, -104.39663493912799, -106.78682465771945, -108.86073554241558, + -110.91272059008197, -113.391986702604, -116.71712944928908, -121.10899616745179, + -126.49101079170191, -132.48890828569083, -138.52907052668536, -143.98403266052748, + -148.28386085159676, -150.950353874693, -151.59029328770967, -149.92372750552684, + -145.8893613284024, -139.79752247093995, -132.44040508370935, -125.04855412827735, + -119.03041856644234, -115.55253894189944, -115.13281650745513, -117.42895405278668, + -121.3137432440983, -125.21048574002899, -127.55581585488525, -127.2035779940535, + -123.63075578035654, -116.92546848468061, -107.64003980195744, -96.63277766107122, + -84.99305975953892, -74.04821594432073, -65.34295582835624, -60.46526291514498, + -60.70407650618115, -66.66922658810824, -78.05651925687684, -93.67617143035707, + -111.7360977518272, -130.24058283704147, -147.30871275663227, -161.30727700805863, + -170.86975350241954, -174.96772570963563, -173.1273631811409, -165.72224178554154, + -154.15911230206518, -140.7708520660487, -128.34356994048002, -119.39667357114564, + -115.49538835769575, -116.87807764072566, -122.52767636389038, -130.62314678287214, + -139.1652566178275, -146.52616783065477, -151.74687442934803, -154.55565073083676, + -155.2031643789819, -154.2459690762812, -152.3741747997027, -150.31243759575744, + -148.7646263574894, -148.35253400075652, -149.52454396025652, -152.4546938221956, + -156.97962048361924, -162.61455065772643, -168.65639907954113, -174.33920996159839, + -178.98123335741235, -182.07734590854741, -183.33592461504546, -182.69582244314955, + -180.3576867141039, -176.82992166677565, -172.94908163552213, -169.81480317042735, + -168.6009806288589, -170.26752614676, -175.26636333032138, -183.36316952799345, + -193.6626336687613, -204.8455532302751, -215.5341659113945, -224.642821518713, -231.5841029386231, + -236.27823723741093, -239.00153643062308, -240.1602207969857, -240.0805222435039, + -238.8792739976468, -236.4402664233786, -232.4857796328019, -226.70748164073063, + -218.90832171524943, -209.10849307704876, -197.58443529231656, -184.83728952594203, + -171.51692947117536, -158.34183621150794, -146.03906486693822, -135.2931551378401, + -126.67274641338989, -120.522356840103, -116.8495422307382, -115.26711775119627, + -115.04262249541723, -115.26672709456207, -115.09929773879223, -114.01174175462747, + -111.938154926336, -109.27967994662575, -106.7610444266074, -105.19243755344766, + -105.22191192914899, -107.15903178753426, -110.91294427768851, -116.03909562366141, + -121.85319340040662, -127.56333385695282, -132.3915434702031, -135.68773579313347, + -137.0553530223522, -136.49152106606573, -134.50396087391331, -132.13096362061216, + -130.78914260327244, -131.92152103045797, -136.50685861312238, -144.58292188907654, + -154.9805743147388, -165.42722647501083, -173.05862816904528, -175.2162591822015, + -170.27334476780416, -158.19760009121686, -140.64912083959248, -120.58743118943534, + -101.54354695989151, -86.82037717684916, -78.86963873202836, -78.9730241819731, + -87.208732905478, -102.59534157990022, -123.30579810686955, -146.90854660058451, + -170.66096479222475, -191.89527725965974, -208.48091570828308, -219.25913002134993, + -224.29508133070183, -224.82455615157096, -222.87658961783313, -220.67901822911378, + -220.0419990907876, -221.92371042747453, -226.30932003382122, -232.4142180953472, + -239.1075434889348, -245.3854703853822, -250.72594095429886, -255.21957879524751, + -259.4638629985364, -264.29101495969917, -270.44645552037593, -278.33363978175873, + -287.8997813229622, -298.675957930859, -309.9292117127586, -320.853465714804, -330.7289512679556, + -339.01001631839443, -345.3404207556757, -349.52468900709295, -351.4950882757702, + -351.30843956420114, -349.19099656641225, -345.6266700588403, -341.4559943792539, + -337.9255863077082, -336.6122458269782, -339.1590394133497, -346.81553649131257, + -359.86602049319964, -377.1262582942168, -395.74022625809545, -411.4675647034107, + -419.50864481032374, -415.70559925146244, -397.7689969926942, -366.1070646451192, + -323.9312171058613, -276.5534397525774, -230.0823638333837, -189.94210230887091, + -159.6860391307984, -140.44018425867142, -131.05960164254728, -128.83405351298265, + -130.4331021797765, -132.77378964958245, -133.598664448043, -131.70104308630897, + -126.85857197498154, -119.59605542060795, -110.89414841971599, -101.91886722614183, + -93.80124103359078, -87.46894981676736, -83.52587581326873, -82.18044665496083, + -83.22666982990846, -86.0776105317449, -89.84325002250512, -93.43992023523293, -95.72026823608788, + -95.61789886265437, -92.30211434757715, -85.33026401582156, -74.77072267985237, + -61.2585993757004, -45.950045468071075, -30.363497680157124, -16.130369251753944, + -4.708318964022113, 2.8777155744834513, 6.212983448101315, 5.56140196148832, 1.82269284521927, + -3.6117451657027027, -9.093677781503999, -13.032649828418581, -14.265052388454068, + -12.419628464737789, -8.183075798478319, -3.351394348965721, -0.5798384940055397, + -2.8238203221278537, -12.574618457960362, -31.09374357688851, -57.88946050977006, + -90.62907055552884, -125.54979971296869, -158.26608815757967, -184.7370925829678, + -202.10993695070283, -209.20930494571166, -206.57443001611037, -196.09246662518265, + -180.3847681302668, -162.1383131743949, -143.5430153470272, -125.92848416505213, + -109.62850194687415, -94.06091918539822, -77.99447435013359, -59.96518435506764, + -38.78528395904798, -14.052481549971988, 13.469116037352999, 41.73558027808582, + 67.67668776494482, 87.82105256949613, 99.17127607438123, 100.09686637149439, 90.96891255317551, + 74.31360110106945, 54.401903491602475, 36.37112195454357, 25.122323308838894, 24.299071767105474, + 35.60733796084545, 58.60925251977363, 90.97083953417878, 129.02527666162507, 168.46442192730262, + 204.9931440177265, 234.84578165082002, 255.13458309044964, 264.04869333102295, 260.9405833712635, + 246.33164088648044, 221.85233074958896, 190.1149237954721, 154.50438006044362, 118.87017882929412, + 87.1135432104481, 62.69333693214143, 48.115270426306004, 44.50773230112809, 51.40175250111006, + 66.8050512534824, 87.59010715789734, 110.12403882647259, 130.98843565029367, 147.60355998106695, + 158.5997478392373, 163.85990646713972, 164.25977605801083, 161.2187754414792, 156.2146660131347, + 150.40095323538046, 144.40924932571875, 138.34575838676523, 131.9293358853309, 124.68710400298332, + 116.12724252945577, 105.83914725773546, 93.51279283344225, 78.90515326791086, 61.80006387437529, + 42.00492201327354, 19.40692541387674, -5.917558252890152, -33.573230874271864, -62.74274528444666, + -92.15503567861164, -120.15265932612857, -144.85998708249542, -164.4286388593254, + -177.31873975901365, -182.56747411912127, -179.99849715708353, -170.33317799853793, + -155.174775508723, -136.8502267694815, -118.11425365797368, -101.74838641351045, + -90.11915445215804, -84.78453470268744, -86.24244721296932, -93.89058890376597, + -106.21495538177916, -121.15951154482048, -136.5743658936049, -150.61621971325263, + -161.99385323020581, -170.00781819684602, -174.40729806224658, -175.15102714454724, + -172.18938784282471, -165.3700625780896, -154.51544144123787, -139.64662444269274, + -121.26379807895945, -100.56065686649099, -79.46407820455939, -60.445349586067294, + -46.126337444272174, -38.77528077288329, -39.82686472674805, -49.55634236027296, + -66.99108245478719, -90.07326313903191, -116.01965740549392, -141.78042183347972, + -164.48915325804674, -181.81824580195223, -192.1936050384186, -194.86464874708207, + -189.85637149914302, -177.8444701746055, -159.9936426666511, -137.78899220364173, + -112.8776101254829, -86.92681328173411, -61.4995443419655, -37.946187885812385, + -17.314095829999818, -0.2793256793802397, 12.892666001859144, 22.35087361058214, + 28.623738724825635, 32.547011711050814, 35.16166362154166, 37.59316158505565, 40.9252515429786, + 46.081001294230944, 53.72209705873961, 64.17527284982758, 77.39288792317001, 92.95301794423807, + 110.10246016313017, 127.84307430057432, 145.05732762285365, 160.66258701121782, + 173.77600795348275, 183.86412276724406, 190.84585708339466, 195.11799782738245, + 197.48122046939216, 198.9638867275426, 200.56755651863108, 202.98585640947218, 206.36735753017484, + 210.1940659607301, 213.32513104699035, 214.21331928977128, 211.25089340990704, 203.15829367329167, + 189.30944456733187, 169.9006771128133, 145.91404235801855, 118.88681324803126, 90.55706188617755, + 62.490765591022935, 35.79722544105233, 11.007111877948589, -11.866681217908095, + -33.12202103984155, -53.09189944897362, -71.89093420571022, -89.25892597900295, + -104.53982239765106, -116.79113539836814, -124.98292837106044, -128.22643422109374, + -125.9727544559038, -118.13810580734831, -105.13616058766276, -87.82215430794395, + -67.3716974347737, -45.12687495280552, -22.4432811717548, -0.5658122278379778, 19.44939304727038, + 36.762171246899385, 50.732584441996885, 60.87993008610584, 66.83886074810738, 68.3326809251376, + 65.17495028403165, 57.29773071625812, 44.792950118379224, 27.94724280533802, 7.252695883249127, + -16.615157231293985, -42.849357139418395, -70.58183967568638, -98.95676356556059, + -127.19340520577649, -154.63036780787843, -180.74870760607394] +yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766, + -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698, + -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024, + -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475, + -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925, + -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358, + -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696, + -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558, + -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959, + -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712, + -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775, + -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227, + -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924, + -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524, + -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154, + -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213, + -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968, + -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885, + -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287, + -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954, + -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699, + -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334, + -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104, + -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568, + -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354, + -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645, + -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636, + -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587, + -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638, + -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972, + -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447, + -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077, + -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827, + -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278, + -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243, + -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458, + -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384, + -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971, + -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393, + -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256, + -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992, + -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318, + -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932, + -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875, + -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126, + -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044, + -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763, + -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265, + -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126, + -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113, + -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113, + -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535, + -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696, + -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247, + -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216, + -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353, + -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494, + -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333, + -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105, + -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883, + -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927, + -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884, + -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401, + -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479, + -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717, + -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403, + -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617, + -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946, + -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314, + -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325, + -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804, + -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694, + -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923, + -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567, + -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941, + -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014, + -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199, + -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829, + -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106, + -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166, + -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521, + -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426, + -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046, + -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633, + -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434, + -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539, + -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053, + -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804, + -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645, + -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914, + -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364, + -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178, + -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894, + -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255, + -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792, + -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684, + -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029, + -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007, + -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605, + -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889, + -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275, + -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017, + -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505, + -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556, + -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376, + -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741, + -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233, + -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081, + -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436, + -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614, + -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242, + -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417, + -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184, + -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262, + -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124, + -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636, + -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339, + -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535, + -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313, + -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217, + -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734, + -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575, + -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034, + -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034, + -420.89393277454684] +z: [8.660695913847082, 66.39862553863676, 127.55801351244567, 180.5416567670594, 210.78937741627567, + 205.98510123498664, 161.75492603580668, 84.86410600259634, -7.938204250916348, -95.46180594158663, + -159.6160710741801, -191.12823315769305, -191.1077383247354, -168.75160790760808, + -136.7976917935674, -106.70965336063549, -85.49283736476575, -74.97102632230975, + -72.98493565768426, -75.44155069558714, -78.2857794410305, -78.76734664689184, -75.79970900381551, + -69.60926545532763, -61.02352026219444, -50.731204460839145, -38.784872157834826, + -24.48063415135472, -6.604904189438815, 16.003062940241094, 43.61116166930451, 74.59937262542272, + 104.69471826704422, 127.10591770220823, 134.08234145821157, 119.62663119956167, + 82.25187144352125, 26.508018246556365, -37.60809657045731, -97.50294343727298, -142.05752447785434, + -165.01189926649766, -166.22428468257075, -150.6580066881632, -125.88440608451583, + -99.27427444087671, -76.01596349902346, -58.51176921704588, -46.89688221845846, + -40.0319509439571, -36.428785825300736, -34.84658879993211, -34.526729879516196, + -35.16461507569286, -36.74598621919215, -39.344955846661044, -42.94609674951552, + -47.33366981479339, -52.08037130315903, -56.64311306447958, -60.522268045019175, + -63.40062822000451, -65.19777275447987, -66.03688501834353, -66.16552673973, -65.87798906520248, + -65.46886057404075, -65.21885341822762, -65.38814742633711, -66.18701058321993, + -67.71310218370519, -69.87490985875417, -72.34304758340024, -74.57596554892733, + -75.9445158717209, -75.92859524663281, -74.30883578638903, -71.26712435663396, -67.34312951723155, + -63.24836193520065, -59.60140045704585, -56.690265423268045, -54.35501112675093, + -52.025856935869335, -48.89272594930154, -44.14172332474001, -37.177555708112656, + -27.766047180552132, -16.071976274482385, -2.617381566487062, 11.77309724198873, + 25.976102073264208, 38.53687298107297, 47.718551492321396, 51.76605423223608, 49.38878979505458, + 40.290429255399275, 25.491354807794263, 7.246430327597227, -11.475853168764028, + -27.772349573580907, -39.515015426681174, -45.713572144488516, -46.46431428981413, + -42.61798316112623, -35.37703866753643, -25.9934677516497, -15.628487382325641, + -5.320192334714672, 4.040136476501501, 11.774699698468378, 17.500102100664304, 21.15959975218581, + 22.97185786614963, 23.31019232970664, 22.558042956454567, 20.99209210295209, 18.729017300170803, + 15.74651164213902, 11.966823265504356, 7.376265857048469, 2.1431266936820315, -3.3126251807763514, + -8.343814311527554, -12.216153231932507, -14.333645421496714, -14.465721023384898, + -12.87582842967554, -10.283561071346963, -7.656458262095206, -5.898235147224569, + -5.548464248913495, -6.607585778672925, -8.55034572367405, -10.517780185318616, + -11.61186892570347, -11.181289206074855, -8.998149061838516, -5.278041737321532, + -0.5603284740797135, 4.485651709130345, 9.246488587633161, 13.28299530174339, 16.36756662020893, + 18.466571978647558, 19.704100343285322, 20.32815531831131, 20.677451138832144, 21.13258932476601, + 22.03831545911599, 23.60334269570648, 25.810407707445663, 28.386041772609907, 30.87468716575818, + 32.83047926730481, 34.087238764628125, 35.01230737770861, 36.621360874602765, 40.45070511179779, + 48.15601919282285, 60.917623297793895, 78.84123893360714, 100.59365670804146, 123.46645783546617, + 143.91789347099578, 158.44750364654655, 164.50260889528977, 161.097954064323, 148.96575759387107, + 130.25873364860564, 107.99215885210386, 85.46056101968273, 65.78797889606493, 51.62771188476158, + 44.92502099696882, 46.66619035735277, 56.63547648764704, 73.29795847361908, 93.94368722527568, + 115.14660187881398, 133.44963208761388, 146.06803902842015, 151.38700573272683, + 149.12923984177337, 140.2151559819424, 126.44367536795221, 110.13915686176617, 93.84830850562516, + 80.0820870481205, 71.04848324787652, 68.34789291609472, 72.6765134206983, 83.63977979446459, + 99.77023650435606, 118.77456269522624, 137.94387073801647, 154.60399035259434, 166.49431469790014, + 172.0327438962138, 170.4928109096748, 162.13218328502208, 148.26316136662118, 131.1890050423139, + 113.90461235170372, 99.51589491572973, 90.46166485727544, 87.7622777984477, 90.58353615476729, + 96.33891906881483, 101.3726007035209, 102.04139409749659, 95.84221536672281, 82.19477689643513, + 62.61034213227201, 40.20548872373059, 18.75852748002458, 1.657957295481827, -8.90300742366735, + -12.266306640477236, -9.163238878856461, -1.233167864960895, 9.564495981683649, + 21.43992022175332, 33.051379335207095, 43.56592729978036, 52.568382283902764, 59.9082904825656, + 65.55991544152265, 69.53869459501789, 71.88526118098649, 72.70284621092118, 72.21843649851635, + 70.83368992994872, 69.13827315439758, 67.87334691224184, 67.8505290321987, 69.8442171675916, + 74.47726967536957, 82.11311705302812, 92.75985141769938, 105.99298201503689, 120.91543676261807, + 136.18772657541675, 150.16300634223884, 161.13869607756567, 167.68973106972928, + 168.99857035076562, 165.07365193284676, 156.7707462316869, 145.59585156800276, 133.34673828809346, + 121.70779304048943, 111.92368383650255, 104.63857613545397, 99.9202392953937, 97.42415696314441, + 96.61636301843436, 96.97307879858319, 98.1025495789197, 99.77349073377508, 101.86901694512392, + 104.30470000982287, 106.95197406963027, 109.5967587731362, 111.94423545513219, 113.66135899930364, + 114.4352894158048, 114.02254875294729, 112.27073376540599, 109.10848671943762, 104.5142402049289, + 98.48427593329139, 91.02225206964965, 82.16482996592228, 72.04340823046537, 60.9644383318998, + 49.47596335726721, 38.38208716267078, 28.675265153703773, 21.380090006656577, 17.336295088765645, + 16.98025827773935, 20.1977232072601, 26.30579134436739, 34.1820237918946, 42.50787279716556, + 50.05364436655743, 55.91974464196498, 59.66868428102189, 61.32427965788826, 61.25993059906409, + 60.02892932141973, 58.19725874007958, 56.22498874652909, 54.41596991142811, 52.9291999133868, + 51.82801400185255, 51.138336589289274, 50.892598938155075, 51.146726044509975, 51.96875114225244, + 53.40564661532219, 55.43870808852528, 57.93823519360839, 60.62739916553108, 63.064899619087015, + 64.65655841637202, 64.7055801259163, 62.50676710339701, 57.47927602641731, 49.31661470793777, + 38.11706927966492, 24.4510077185352, 9.330391041091739, -5.928968195692799, -19.927807358089126, + -31.432334046946572, -39.58564206883777, -44.02742340146397, -44.89862410201391, + -42.74448812517229, -38.35747836190584, -32.61145550528262, -26.32954437918894, + -20.20719977067912, -14.789151242435427, -10.48250380540003, -7.582119063177211, + -6.287826913269372, -6.702621306839947, -8.81253570937736, -12.45851173987955, -17.315467703254903, + -22.89273066518226, -28.563759038476075, -33.62421205979971, -37.369384313159216, + -39.17768128465537, -38.587042278968454, -35.35458955341083, -29.49368865852931, + -21.285110785761752, -11.260045417067115, -0.15386876206445768, 11.167509802528333, + 21.80199162922029, 30.914335137198602, 37.83281681446311, 42.1250528920106, 43.6424129336149, + 42.527536031710504, 39.1838083377227, 34.20896823571095, 28.298619788384883, 22.130902226323823, + 16.250738211119412, 10.978234041445447, 6.366580287176825, 2.2267366046830612, -1.780708650958427, + -6.007319377392964, -10.670867462380551, -15.74701218807577, -20.935394624211764, + -25.71475763924228, -29.473174580935297, -31.674804797246836, -32.01284071766054, + -30.503625724620882, -27.49687795069467, -23.603695718045664, -19.567702223835404, + -16.117560259075034, -13.838347771722535, -13.087536620644972, -13.9646827725496, + -16.328965936199715, -19.84970900489466, -24.07290465216959, -28.48977493682292, + -32.59854442331317, -35.95539956520115, -38.2137508958304, -39.15228104770502, -38.692332219912934, + -36.90456601359803, -34.004068329378526, -30.332692841352, -26.327899158206776, + -22.478863588907455, -19.27298194233058, -17.13832959626637, -16.389291255919126, + -17.18279456348231, -19.4913537172405, -23.09696936219348, -27.60749078959273, -32.494672428897246, + -37.15076766150979, -40.95788530079359, -43.36162493187602, -43.93846520775633, + -42.446152023679346, -38.848786666305784, -33.313331043792914, -26.180492183033934, + -17.918310843908614, -9.069298172978261, -0.20074671809499756, 8.136492955152297, + 15.441031108092213, 21.29036758626846, 25.372508549246056, 27.522718958733332, 27.762289494620244, + 26.32911239456066, 23.684885172768034, 20.483910329661786, 17.495388855951955, 15.484035434486861, + 15.069215691030688, 16.59509470023471, 20.047924906971748, 25.048393934050562, 30.927904484071945, + 36.87324018466698, 42.10253497425529, 46.024752154331864, 48.33939153393884, 49.0517871424882, + 48.40580399084933, 46.76081807159332, 44.45507771693884, 41.69825116535199, 38.52289882103032, + 34.803261444171405, 30.327999320714078, 24.898546537842716, 18.420586405729864, + 10.962638364832946, 2.769145218537321, -5.769675941714455, -14.177570162462116, + -21.96287190097139, -28.682775714701705, -33.98993062535793, -37.65744218731808, + -39.584533342726225, -39.78848208739123, -38.38883996706238, -35.58823507210259, + -31.651659030216656, -26.884305680876444, -21.60752145164577, -16.133301088860883, + -10.739449278450719, -5.649094917111724, -1.018762093669203, 3.061876393936713, + 6.558023261640048, 9.468369818928435, 11.800068844557336, 13.547269650304042, 14.679655966520992, + 15.144569931188034, 14.882211925027542, 13.849398415516339, 12.04474239771272, 9.52770863530006, + 6.425839876890219, 2.9278589763920806, -0.735815798162439, -4.320509611419369, -7.596486438809738, + -10.37389619761122, -12.519116524619744, -13.960610192047202, -14.685038419335728, + -14.726561926961596, -14.153639370214792, -13.057964531219815, -11.549353043113381, + -9.758421987878664, -7.846069969532445, -6.015682302494043, -4.521627491850837, + -3.6670759284008367, -3.78622388922754, -5.210529940531722, -8.224269075041521, + -13.019390902549098, -19.661194514880624, -28.07379403356179, -38.04868886403423, + -49.2733259570762, -61.37184021856482, -73.94857243204568, -86.62633925391154, -99.07453382284537, + -111.02550604149512] +pitch: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766, + -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698, + -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024, + -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475, + -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925, + -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358, + -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696, + -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558, + -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959, + -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712, + -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775, + -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227, + -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924, + -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524, + -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154, + -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213, + -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968, + -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885, + -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287, + -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954, + -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699, + -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334, + -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104, + -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568, + -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354, + -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645, + -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636, + -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587, + -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638, + -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972, + -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447, + -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077, + -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827, + -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278, + -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243, + -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458, + -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384, + -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971, + -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393, + -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256, + -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992, + -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318, + -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932, + -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875, + -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126, + -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044, + -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763, + -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265, + -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126, + -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113, + -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113, + -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535, + -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696, + -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247, + -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216, + -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353, + -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494, + -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333, + -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105, + -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883, + -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927, + -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884, + -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401, + -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479, + -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717, + -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403, + -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617, + -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946, + -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314, + -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325, + -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804, + -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694, + -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923, + -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567, + -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941, + -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014, + -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199, + -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829, + -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106, + -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166, + -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521, + -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426, + -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046, + -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633, + -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434, + -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539, + -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053, + -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804, + -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645, + -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914, + -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364, + -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178, + -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894, + -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255, + -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792, + -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684, + -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029, + -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007, + -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605, + -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889, + -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275, + -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017, + -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505, + -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556, + -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376, + -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741, + -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233, + -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081, + -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436, + -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614, + -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242, + -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417, + -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184, + -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262, + -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124, + -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636, + -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339, + -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535, + -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313, + -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217, + -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734, + -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575, + -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034, + -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034, + -420.89393277454684] +roll: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766, + -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698, + -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024, + -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475, + -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925, + -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358, + -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696, + -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558, + -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959, + -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712, + -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775, + -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227, + -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924, + -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524, + -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154, + -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213, + -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968, + -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885, + -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287, + -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954, + -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699, + -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334, + -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104, + -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568, + -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354, + -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645, + -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636, + -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587, + -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638, + -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972, + -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447, + -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077, + -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827, + -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278, + -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243, + -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458, + -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384, + -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971, + -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393, + -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256, + -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992, + -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318, + -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932, + -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875, + -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126, + -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044, + -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763, + -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265, + -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126, + -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113, + -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113, + -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535, + -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696, + -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247, + -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216, + -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353, + -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494, + -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333, + -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105, + -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883, + -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927, + -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884, + -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401, + -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479, + -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717, + -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403, + -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617, + -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946, + -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314, + -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325, + -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804, + -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694, + -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923, + -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567, + -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941, + -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014, + -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199, + -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829, + -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106, + -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166, + -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521, + -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426, + -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046, + -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633, + -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434, + -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539, + -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053, + -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804, + -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645, + -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914, + -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364, + -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178, + -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894, + -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255, + -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792, + -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684, + -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029, + -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007, + -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605, + -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889, + -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275, + -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017, + -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505, + -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556, + -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376, + -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741, + -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233, + -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081, + -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436, + -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614, + -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242, + -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417, + -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184, + -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262, + -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124, + -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636, + -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339, + -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535, + -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313, + -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217, + -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734, + -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575, + -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034, + -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034, + -420.89393277454684] +x: [-131.00261478679988, -121.88886417422353, -111.9050389833139, -102.61368963497223, + -96.05947080078046, -94.12535169815399, -97.8607435820271, -107.14613364957096, + -120.82659085972966, -137.19720078410953, -154.5163549178483, -171.18694825080595, + -185.5568218223094, -195.704691059387, -199.6329868378576, -195.91936207338776, + -184.48915098542318, -167.07793507920195, -147.0574840422674, -128.53128958566316, + -115.04358889413827, -108.56400855374055, -109.19561099927448, -115.57413755530115, + -125.64926844955686, -137.46655539270932, -149.61816709281686, -161.26501144566066, + -171.91636446335687, -181.2216960780747, -188.87007846616464, -194.53289513944736, + -197.79145167358277, -198.0996571089822, -194.8802396222356, -187.78401698176944, + -177.02826515891735, -163.64756841648978, -149.46450567572836, -136.67890898964777, + -127.21395723237792, -122.15440349976664, -121.54367242044958, -124.56838026444774, + -129.9847428341204, -136.57776680869415, -143.4619977851926, -150.1463172881614, + -156.43047062938794, -162.25579819785062, -167.580799896415, -172.2857363783464, + -176.10313080152142, -178.60602094331426, -179.29466139771722, -177.77366590562514, + -173.94835807935246, -168.14763364573156, -161.10576879062413, -153.79437054295704, + -147.18005615509452, -142.03544698559352, -138.8812834694729, -138.0254327793118, + -139.60154077046317, -143.53795283987066, -149.46190564030064, -156.61307764750555, + -163.87292592523232, -169.97827533844975, -173.87402597845175, -175.0512378675456, + -173.7109894894428, -170.68112972530366, -167.11714893622806, -164.10386427943158, + -162.31622495964814, -161.86114761154576, -162.33151047253835, -163.02192881838113, + -163.2095501914243, -162.39248606696069, -160.40951035619491, -157.42206476020834, + -153.79159590063577, -149.91799716532543, -146.1123968434442, -142.55104990302257, + -139.31149673683075, -136.46131741930625, -134.16539879470983, -132.7805579230332, + -132.8976720404706, -135.27842864395015, -140.64645739243105, -149.3508159834204, + -161.00381971456645, -174.2605798886825, -186.9134811438676, -196.38261868432437, + -200.49211090980378, -198.23763830134754, -190.21480773205266, -178.51115567595826, + -166.0801280692791, -155.83164235826004, -149.80028431814193, -148.6986554765198, + -151.96202945833986, -158.18027510739103, -165.68937034330935, -173.07927043758525, + -179.46192687105753, -184.482441275634, -188.1614046703995, -190.6833573748501, + -192.21809683888495, -192.81960540582097, -192.41403431825069, -190.86577715074623, + -188.09301880254696, -184.18878513004086, -179.4955981906823, -174.5892113462465, + -170.1561255168205, -166.79776163996192, -164.8373602238494, -164.2126907946925, + -164.50294621069565, -165.0839384166419, -165.3557930296701, -164.95884643563477, + -163.898412256587, -162.534820649653, -161.44472376807636, -161.20582442689792, + -162.18450627688458, -164.40124437891336, -167.51392693816024, -170.91206169710102, + -173.8749652264376, -175.73083709030058, -175.97063351490164, -174.31004528380663, + -170.72511032331406, -165.48897950595216, -159.20946482285498, -152.82808750163824, + -147.5191667738631, -144.44886967752777, -144.4240273588793, -147.54449659368146, + -153.01617480666846, -159.25176211818498, -164.2877168274527, -166.41140144219656, + -164.78014816559207, -159.78674905727087, -153.00534915065177, -146.70549554069234, + -143.09178186665255, -143.5446658872191, -148.14637102277132, -155.6663204124734, + -164.00416538606794, -170.916565992789, -174.75161613413627, -174.9259359923934, + -171.99663265443107, -167.34336788125873, -162.6147423868518, -159.16048671989986, + -157.6477458947188, -157.96425018036857, -159.39300876091175, -160.95217491299363, + -161.7572333232152, -161.28442609094157, -159.4763125555903, -156.69932273396668, + -153.6090737832136, -150.9888364227075, -149.60413892450302, -150.0809012374839, + -152.79153608790728, -157.7397093479904, -164.46467355593433, -172.0172934256874, + -179.0661158741304, -184.15783677719142, -186.08855140295393, -184.27238379689192, + -178.96525805487465, -171.2372865037682, -162.67650112956372, -154.91277319136154, + -149.12741792008617, -145.72381647659327, -144.27134285886908, -143.72945580899287, + -142.85723879551645, -140.65447875831518, -136.68276519662683, -131.17312406130938, + -124.91209567953047, -118.9751543144055, -114.4168612960534, -112.01955243552163, + -112.15689719095518, -114.77332263747567, -119.44360198084003, -125.471236872446, + -132.00253791160844, -138.1561515203214, -143.17484043177828, -146.58907912429945, + -148.3511438380617, -148.87742279194816, -148.94591893398302, -149.43982973654823, + -150.99302147422435, -153.65215284576814, -156.69307614107936, -158.6972973355033, + -157.91096745292066, -152.8004175957856, -142.62726270976202, -127.83568739093043, + -110.09574251961111, -91.96442744166842, -76.26517339306562, -65.38756088681923, + -60.727455422828086, -62.41769838627875, -69.38103902919622, -79.63028303419432, + -90.6904492501862, -100.0330543381811, -105.46650890222921, -105.47370228165889, + -99.49440285791094, -88.11732926351014, -73.10854974665124, -57.20048636703596, + -43.619919989560685, -35.42935377805215, -34.84789242860901, -42.75227348465364, + -58.50946410168745, -80.17821602052324, -104.99065766337972, -129.94273353227604, + -152.31614487459336, -170.01709776381944, -181.70800379508364, -186.78019457154642, + -185.24347046638363, -177.59665180543396, -164.71393092910603, -147.75511927137686, + -128.09259386421706, -107.2415258381779, -86.77724029789665, -68.22375027449615, + -52.90610923413581, -41.779927174503385, -35.27752870209294, -33.22579478747385, + -34.88146090859914, -39.094144062824924, -44.55933540558859, -50.084953618202576, + -54.78496998250598, -58.13877584276208, -59.90622409641475, -59.945540069741, -58.02266713890794, + -53.710211558038985, -46.44646056518766, -35.76628477961155, -21.64346099537084, + -4.823099943406338, 13.00074336490228, 29.276996809341675, 40.992025758372115, 45.29133769691425, + 40.16014663983825, 24.958013623469462, 0.6430403690297408, -30.37648983818459, -64.70364605521273, + -98.5780766753714, -128.49010205980096, -151.64916889145232, -166.24865624155242, + -171.54102444522545, -167.77666186080413, -156.06265128020192, -138.17902607586348, + -116.36917642692488, -93.10932192371236, -70.85982959536072, -51.803566443571675, + -37.58102962803787, -29.04191085373232, -26.05140387409622, -27.411942444947794, + -30.971298702656785, -33.967139701487234, -33.59780771242022, -27.721946232536823, + -15.50944865087424, 2.165242029053751, 22.75109265450623, 42.51462881068035, 57.39178500711525, + 63.9603626762866, 60.24680348570627, 46.13482203280333, 23.280449691574223, -5.396531697017779, + -36.44546991778746, -66.59572190185905, -93.244393312115, -114.63002544381644, -129.7277803714285, + -137.99626292368302, -139.13919014515983, -133.01230035499455, -119.72497935278233, + -99.88864280213238, -74.88535670756845, -46.99987777401981, -19.290622507018494, + 4.836900082983619, 22.275753850864515, 30.9051736945987, 30.102091149122813, 20.89767984126534, + 5.709426552642763, -12.280539670085691, -29.912602539696113, -44.76409817954616, + -55.587777836517255, -62.37400184312159, -66.06509298635545, -68.05618356119531, + -69.6650415443935, -71.73438308006683, -74.46044693767506, -77.45330286152975, -79.96093287153582, + -81.15272296993173, -80.36351741010003, -77.23600916293604, -71.74756168638527, + -64.14841195780141, -54.85947253693819, -44.377179252428895, -33.21572713174938, + -21.893453397774184, -10.949679527408357, -0.9673128756345823, 7.423219742995925, + 13.573678927669173, 16.882316200266693, 16.880518451590333, 13.321463895553073, + 6.2494544216467425, -3.9662652022608986, -16.64105804227179, -30.827458079441, -45.40427423165941, + -59.19176164715269, -71.07926973763034, -80.1486617898492, -85.77441240031021, -87.6818651318595, + -85.95088814283162, -80.96333944498441, -73.30666637565525, -63.65782924774226, + -52.676828368491805, -40.93506347992221, -28.891766117399765, -16.916080004322772, + -5.3384835550584935, 5.4924058263232025, 15.171162899221802, 23.231051427618734, + 29.178380505050853, 32.559117153019926, 33.042995254450894, 30.506717141912656, + 25.098309221713915, 17.267877434957573, 7.754667766136128, -2.4738852870074073, + -12.328160893560188, -20.73549791063537, -26.79110541102302, -29.892155231726377, + -29.825103487837175, -26.78324113785426, -21.306403690118064, -14.15394397582623, + -6.139545262622449, 2.0335265366227753, 9.89870331065091, 17.286397543623664, 24.307308641748374, + 31.26525601401877, 38.52026696289829, 46.33161129748563, 54.71416332554038, 63.34061174435135, + 71.51679336934421, 78.24704133338132, 82.39018658725793, 82.88576123321361, 79.00811170033668, + 70.59040844420252, 58.15831519340492, 42.928737491281964, 26.660959489767816, 11.387156505595188, + -0.9161323493847441, -8.636955778598946, -10.746100985455664, -6.88302166146115, + 2.6901550272644528, 17.227609362490647, 35.67099777743684, 56.793973316018906, 79.28492204264502, + 101.76799145249652, 122.79490061004348, 140.8566568494178, 154.45830822835748, 162.2730575641024, + 163.35441171879694, 157.35115570417733, 144.6537822686842, 126.41052162787045, 104.38486553091724, + 80.67370587471055, 57.34955312756698, 36.11589971154659, 18.06234868471712, 3.576456180471944, + -7.5771785605298545, -16.047264197592554, -22.625071287068153, -27.99125205987563, + -32.544187050649704, -36.342770320132814, -39.160889188810124, -40.621845381417664, + -40.36566724147911, -38.20206823583871, -34.21329090178294, -28.788359919239177, + -22.587615544563256, -16.449965238654954, -11.263594052219574, -7.82420704968046, + -6.7043106382969215, -8.153633537426817, -12.045315570129013, -17.87558753135798, + -24.817072522162295, -31.818486356751524, -37.73735081273553, -41.48808157884077, + -42.185843095170476, -39.266966337888135, -32.56948414179687, -22.362390213218752, + -9.319317728622229, 5.559271102094557, 21.062973392548795, 35.90860732266084, 48.87612268299202, + 58.92359238196129, 65.26758898268541, 67.42353430763573, 65.20869447959286, 58.716140928984274, + 48.27012084952794, 34.372265068707556, 17.645386644363096, -1.221017147956034, -21.52101235133283, + -42.57613526226664, -63.77089174961588, -84.5781075128901, -104.57331194568057, + -123.43864361815416, -140.9579908724867, -157.005848574077] +y: [-99.19714419940159, -79.16394233266908, -57.87218080909418, -39.28438472983111, + -28.386800564847263, -29.413961436687575, -43.913473953576144, -69.67422837210728, + -101.14133069492664, -131.32148548272554, -154.36897298960562, -167.56000537729983, + -171.8480811942197, -171.05140505400772, -170.15303380273187, -173.34724789149277, + -182.51652550260764, -196.6077612179733, -212.0578803921079, -224.1551729270074, + -228.85174829123696, -224.25993586587964, -211.25206790585622, -193.0143316349947, + -173.74730833399678, -157.0391212367104, -144.63181352345478, -136.04353714588737, + -129.03535351100325, -120.64812612176986, -108.4578159331508, -91.66712058998779, + -71.6845532860305, -51.92454636690193, -36.75664559094005, -29.944229192958293, + -33.26253995101549, -45.8841436370077, -64.72238023555612, -85.55581883977085, -104.43639262659643, + -118.73440682883057, -127.4439469965765, -130.85855117578427, -129.9883150575577, + -126.05068565061471, -120.20491993068747, -113.51752511264922, -107.00751318510588, + -101.6120221067554, -98.037143648059, -96.5901783209796, -97.11117132586354, -99.05464430683851, + -101.69784273360709, -104.39663493912799, -106.78682465771945, -108.86073554241558, + -110.91272059008197, -113.391986702604, -116.71712944928908, -121.10899616745179, + -126.49101079170191, -132.48890828569083, -138.52907052668536, -143.98403266052748, + -148.28386085159676, -150.950353874693, -151.59029328770967, -149.92372750552684, + -145.8893613284024, -139.79752247093995, -132.44040508370935, -125.04855412827735, + -119.03041856644234, -115.55253894189944, -115.13281650745513, -117.42895405278668, + -121.3137432440983, -125.21048574002899, -127.55581585488525, -127.2035779940535, + -123.63075578035654, -116.92546848468061, -107.64003980195744, -96.63277766107122, + -84.99305975953892, -74.04821594432073, -65.34295582835624, -60.46526291514498, + -60.70407650618115, -66.66922658810824, -78.05651925687684, -93.67617143035707, + -111.7360977518272, -130.24058283704147, -147.30871275663227, -161.30727700805863, + -170.86975350241954, -174.96772570963563, -173.1273631811409, -165.72224178554154, + -154.15911230206518, -140.7708520660487, -128.34356994048002, -119.39667357114564, + -115.49538835769575, -116.87807764072566, -122.52767636389038, -130.62314678287214, + -139.1652566178275, -146.52616783065477, -151.74687442934803, -154.55565073083676, + -155.2031643789819, -154.2459690762812, -152.3741747997027, -150.31243759575744, + -148.7646263574894, -148.35253400075652, -149.52454396025652, -152.4546938221956, + -156.97962048361924, -162.61455065772643, -168.65639907954113, -174.33920996159839, + -178.98123335741235, -182.07734590854741, -183.33592461504546, -182.69582244314955, + -180.3576867141039, -176.82992166677565, -172.94908163552213, -169.81480317042735, + -168.6009806288589, -170.26752614676, -175.26636333032138, -183.36316952799345, + -193.6626336687613, -204.8455532302751, -215.5341659113945, -224.642821518713, -231.5841029386231, + -236.27823723741093, -239.00153643062308, -240.1602207969857, -240.0805222435039, + -238.8792739976468, -236.4402664233786, -232.4857796328019, -226.70748164073063, + -218.90832171524943, -209.10849307704876, -197.58443529231656, -184.83728952594203, + -171.51692947117536, -158.34183621150794, -146.03906486693822, -135.2931551378401, + -126.67274641338989, -120.522356840103, -116.8495422307382, -115.26711775119627, + -115.04262249541723, -115.26672709456207, -115.09929773879223, -114.01174175462747, + -111.938154926336, -109.27967994662575, -106.7610444266074, -105.19243755344766, + -105.22191192914899, -107.15903178753426, -110.91294427768851, -116.03909562366141, + -121.85319340040662, -127.56333385695282, -132.3915434702031, -135.68773579313347, + -137.0553530223522, -136.49152106606573, -134.50396087391331, -132.13096362061216, + -130.78914260327244, -131.92152103045797, -136.50685861312238, -144.58292188907654, + -154.9805743147388, -165.42722647501083, -173.05862816904528, -175.2162591822015, + -170.27334476780416, -158.19760009121686, -140.64912083959248, -120.58743118943534, + -101.54354695989151, -86.82037717684916, -78.86963873202836, -78.9730241819731, + -87.208732905478, -102.59534157990022, -123.30579810686955, -146.90854660058451, + -170.66096479222475, -191.89527725965974, -208.48091570828308, -219.25913002134993, + -224.29508133070183, -224.82455615157096, -222.87658961783313, -220.67901822911378, + -220.0419990907876, -221.92371042747453, -226.30932003382122, -232.4142180953472, + -239.1075434889348, -245.3854703853822, -250.72594095429886, -255.21957879524751, + -259.4638629985364, -264.29101495969917, -270.44645552037593, -278.33363978175873, + -287.8997813229622, -298.675957930859, -309.9292117127586, -320.853465714804, -330.7289512679556, + -339.01001631839443, -345.3404207556757, -349.52468900709295, -351.4950882757702, + -351.30843956420114, -349.19099656641225, -345.6266700588403, -341.4559943792539, + -337.9255863077082, -336.6122458269782, -339.1590394133497, -346.81553649131257, + -359.86602049319964, -377.1262582942168, -395.74022625809545, -411.4675647034107, + -419.50864481032374, -415.70559925146244, -397.7689969926942, -366.1070646451192, + -323.9312171058613, -276.5534397525774, -230.0823638333837, -189.94210230887091, + -159.6860391307984, -140.44018425867142, -131.05960164254728, -128.83405351298265, + -130.4331021797765, -132.77378964958245, -133.598664448043, -131.70104308630897, + -126.85857197498154, -119.59605542060795, -110.89414841971599, -101.91886722614183, + -93.80124103359078, -87.46894981676736, -83.52587581326873, -82.18044665496083, + -83.22666982990846, -86.0776105317449, -89.84325002250512, -93.43992023523293, -95.72026823608788, + -95.61789886265437, -92.30211434757715, -85.33026401582156, -74.77072267985237, + -61.2585993757004, -45.950045468071075, -30.363497680157124, -16.130369251753944, + -4.708318964022113, 2.8777155744834513, 6.212983448101315, 5.56140196148832, 1.82269284521927, + -3.6117451657027027, -9.093677781503999, -13.032649828418581, -14.265052388454068, + -12.419628464737789, -8.183075798478319, -3.351394348965721, -0.5798384940055397, + -2.8238203221278537, -12.574618457960362, -31.09374357688851, -57.88946050977006, + -90.62907055552884, -125.54979971296869, -158.26608815757967, -184.7370925829678, + -202.10993695070283, -209.20930494571166, -206.57443001611037, -196.09246662518265, + -180.3847681302668, -162.1383131743949, -143.5430153470272, -125.92848416505213, + -109.62850194687415, -94.06091918539822, -77.99447435013359, -59.96518435506764, + -38.78528395904798, -14.052481549971988, 13.469116037352999, 41.73558027808582, + 67.67668776494482, 87.82105256949613, 99.17127607438123, 100.09686637149439, 90.96891255317551, + 74.31360110106945, 54.401903491602475, 36.37112195454357, 25.122323308838894, 24.299071767105474, + 35.60733796084545, 58.60925251977363, 90.97083953417878, 129.02527666162507, 168.46442192730262, + 204.9931440177265, 234.84578165082002, 255.13458309044964, 264.04869333102295, 260.9405833712635, + 246.33164088648044, 221.85233074958896, 190.1149237954721, 154.50438006044362, 118.87017882929412, + 87.1135432104481, 62.69333693214143, 48.115270426306004, 44.50773230112809, 51.40175250111006, + 66.8050512534824, 87.59010715789734, 110.12403882647259, 130.98843565029367, 147.60355998106695, + 158.5997478392373, 163.85990646713972, 164.25977605801083, 161.2187754414792, 156.2146660131347, + 150.40095323538046, 144.40924932571875, 138.34575838676523, 131.9293358853309, 124.68710400298332, + 116.12724252945577, 105.83914725773546, 93.51279283344225, 78.90515326791086, 61.80006387437529, + 42.00492201327354, 19.40692541387674, -5.917558252890152, -33.573230874271864, -62.74274528444666, + -92.15503567861164, -120.15265932612857, -144.85998708249542, -164.4286388593254, + -177.31873975901365, -182.56747411912127, -179.99849715708353, -170.33317799853793, + -155.174775508723, -136.8502267694815, -118.11425365797368, -101.74838641351045, + -90.11915445215804, -84.78453470268744, -86.24244721296932, -93.89058890376597, + -106.21495538177916, -121.15951154482048, -136.5743658936049, -150.61621971325263, + -161.99385323020581, -170.00781819684602, -174.40729806224658, -175.15102714454724, + -172.18938784282471, -165.3700625780896, -154.51544144123787, -139.64662444269274, + -121.26379807895945, -100.56065686649099, -79.46407820455939, -60.445349586067294, + -46.126337444272174, -38.77528077288329, -39.82686472674805, -49.55634236027296, + -66.99108245478719, -90.07326313903191, -116.01965740549392, -141.78042183347972, + -164.48915325804674, -181.81824580195223, -192.1936050384186, -194.86464874708207, + -189.85637149914302, -177.8444701746055, -159.9936426666511, -137.78899220364173, + -112.8776101254829, -86.92681328173411, -61.4995443419655, -37.946187885812385, + -17.314095829999818, -0.2793256793802397, 12.892666001859144, 22.35087361058214, + 28.623738724825635, 32.547011711050814, 35.16166362154166, 37.59316158505565, 40.9252515429786, + 46.081001294230944, 53.72209705873961, 64.17527284982758, 77.39288792317001, 92.95301794423807, + 110.10246016313017, 127.84307430057432, 145.05732762285365, 160.66258701121782, + 173.77600795348275, 183.86412276724406, 190.84585708339466, 195.11799782738245, + 197.48122046939216, 198.9638867275426, 200.56755651863108, 202.98585640947218, 206.36735753017484, + 210.1940659607301, 213.32513104699035, 214.21331928977128, 211.25089340990704, 203.15829367329167, + 189.30944456733187, 169.9006771128133, 145.91404235801855, 118.88681324803126, 90.55706188617755, + 62.490765591022935, 35.79722544105233, 11.007111877948589, -11.866681217908095, + -33.12202103984155, -53.09189944897362, -71.89093420571022, -89.25892597900295, + -104.53982239765106, -116.79113539836814, -124.98292837106044, -128.22643422109374, + -125.9727544559038, -118.13810580734831, -105.13616058766276, -87.82215430794395, + -67.3716974347737, -45.12687495280552, -22.4432811717548, -0.5658122278379778, 19.44939304727038, + 36.762171246899385, 50.732584441996885, 60.87993008610584, 66.83886074810738, 68.3326809251376, + 65.17495028403165, 57.29773071625812, 44.792950118379224, 27.94724280533802, 7.252695883249127, + -16.615157231293985, -42.849357139418395, -70.58183967568638, -98.95676356556059, + -127.19340520577649, -154.63036780787843, -180.74870760607394] +yaw: [-156.65718192265612, -156.82512813476856, -157.03476785128979, -157.2807477120766, + -157.55369295460645, -157.84461759860156, -158.14778763059294, -158.459737944698, + -158.77686003690096, -159.09532479965736, -159.41325989374954, -159.73163715474024, + -160.0521375845057, -160.3745530176387, -160.69700732848457, -161.0185104942475, + -161.3402653767862, -161.66397674111627, -161.98953153448417, -162.315201866925, + -162.64003295077077, -162.96511555690367, -163.29205326830424, -163.62078022025358, + -163.9496971026068, -164.27788257902984, -164.60632279261657, -164.93652376742696, + -165.26846111422094, -165.60065680035294, -165.93222401150882, -166.26405206032558, + -166.5975532366255, -166.9327393757321, -167.2682466574162, -167.60322357479959, + -167.93847000443222, -168.27530835178302, -168.61378183900877, -168.95263403217712, + -169.29104925446183, -169.62974494472823, -169.9699574813401, -170.31175703012775, + -170.6539879618333, -170.99587071205337, -171.33804689251744, -171.68167070253227, + -172.02683518428148, -172.37247918025702, -172.71785930237908, -173.06354756695924, + -173.4106198177865, -173.75918826314913, -174.10828013599635, -174.45718809095524, + -174.8064204116356, -175.1569783712803, -175.50898997238295, -175.86156501042154, + -176.21403187168687, -176.56684061134436, -176.9209216656706, -177.276415779213, + -177.63250973601328, -177.9885671847553, -178.34498510912093, -178.7026267789968, + -179.0616429301755, -179.42129201479344, -179.7809723347144, -180.14103262348885, + -180.50227258176474, -180.86485046896803, -181.2280913368986, -181.59142740879287, + -181.95516366594228, -182.3200397542152, -182.6862192544363, -183.0530889992954, + -183.42011429540196, -183.78756055865932, -184.15611080378343, -184.525931978699, + -184.8964681246396, -185.2672167028437, -185.638407452449, -186.01067008275334, + -186.3841731854121, -186.75841368027824, -187.132920178221, -187.50789034493104, + -187.88390380611247, -188.26112928818085, -188.6391124973969, -189.01741212654568, + -189.39619709895, -189.7760000696095, -190.15698858912273, -190.53875329031354, + -190.92088183004392, -191.30351746122224, -191.68714886802226, -192.07194129758645, + -192.4575266759198, -192.8435204676572, -193.23004308121793, -193.617542113636, + -194.0061795490326, -194.39562519327265, -194.78552113473754, -195.17596753027587, + -195.56737365493905, -195.95989742408074, -196.35324332333818, -196.74707886293638, + -197.14148632095296, -197.53683929553736, -197.933290967728, -198.33057750888972, + -198.7283906402854, -199.12679692660544, -199.52613681329163, -199.92655820874447, + -200.32782617456402, -200.72965543146975, -201.13209880120448, -201.53546597968077, + -201.93989917925003, -202.34518974707686, -202.751074198292, -203.1575933993827, + -203.56502857939287, -203.97351593447695, -204.38287067560304, -204.7928499203278, + -205.20348419671345, -205.6150284301484, -206.0276125727257, -206.44107345232243, + -206.85518761577225, -207.26997671022124, -207.68567140275596, -208.10239525551458, + -208.5200046331361, -208.93829436247796, -209.35727851912242, -209.77716544140384, + -210.19807222793216, -210.61987285855759, -211.04237931918513, -211.4655992857971, + -211.8897205841901, -212.3148538391931, -212.74088887477996, -213.16765374694393, + -213.5951507769902, -214.02354898389035, -214.45295256340523, -214.8832655549256, + -215.31433103073036, -215.74614688524275, -216.1788649289692, -216.61258302054992, + -217.04721792048107, -217.48262670125752, -217.91880365055158, -218.3558848648318, + -218.79396199767987, -219.23296316292127, -219.67275845698202, -220.11333928225932, + -220.5548274153216, -220.99730847034053, -221.44072066552712, -221.88494618630875, + -222.32997418117233, -222.77591340446227, -223.22284362421576, -223.67071202540126, + -224.119411989995, -224.56893096190808, -225.01936587844727, -225.4707908770044, + -225.9231610756851, -226.37638020375584, -226.83043447547186, -227.2854101278763, + -227.7413759005292, -228.1982939079816, -228.65607742107326, -229.11471183206265, + -229.57427371024147, -230.0348266430826, -230.4963388949898, -230.95873251621126, + -231.42199242410942, -231.88618647266296, -232.35137335201267, -232.81752671335113, + -233.2845766674393, -233.7525079495373, -234.22138057487498, -234.69124859655113, + -235.16209036671597, -235.6338433804674, -236.1064924352654, -236.58009051246535, + -237.05468729088872, -237.53026520903265, -238.00676851209414, -238.48418226093696, + -238.9625531403663, -239.44192671749772, -239.92228896806165, -240.40359029407247, + -240.88581618288202, -241.3690076965991, -241.85320655070794, -242.33840176912216, + -242.8245493571952, -243.31163535831513, -243.79969582627467, -244.2887688805353, + -244.77884615907135, -245.26988875560264, -245.7618833697687, -246.2548616058494, + -246.74885823676817, -247.2438671305236, -247.73985399131797, -248.23680624976333, + -248.7347515676374, -249.23372161331298, -249.73371214631064, -250.2346930390105, + -250.73665250571727, -251.23961472458112, -251.74360849280094, -252.2486311641883, + -252.75465637099336, -253.2616731450968, -253.7697025952815, -254.27877087145927, + -254.78887666179244, -255.29999698245618, -255.81212170080903, -256.3252692292884, + -256.8394632842479, -257.3547036618484, -257.87097041693903, -258.3882542568401, + -258.9065712326523, -259.4259428302655, -259.94636975763615, -260.4678347920479, + -260.99032947413946, -261.5138677937398, -262.0384691984253, -262.564135138717, + -263.09085082541867, -263.6186086167544, -264.14742070931345, -264.677304693403, + -265.208262616922, -265.7402818609308, -266.27335557821664, -266.807494410878, -267.3427142618617, + -267.8790176526088, -268.4163938951752, -268.9548369081826, -269.4943559912946, + -270.0349655189502, -270.5766683811865, -271.11945560417905, -271.6633218393314, + -272.20827523166446, -272.75432877508274, -273.3014856399149, -273.84973837039325, + -274.3990823145224, -274.94952462848454, -275.50107706299923, -276.0537429949804, + -276.6075163099427, -277.16239301421695, -277.7183794210768, -278.27548616510694, + -278.8337167688488, -279.39306630014715, -279.95353138416385, -280.5151176192923, + -281.07783464110923, -281.64168606790315, -282.2066680073117, -282.77277766335567, + -283.3400200314943, -283.90840385592054, -284.47793281036434, -285.0486039147941, + -285.6204149122557, -286.19337029282093, -286.7674780078702, -287.3427417545014, + -287.91915935135097, -288.4967290413007, -289.07545489372984, -289.655344157199, + -290.2364005271319, -290.81862251976554, -291.40200883968265, -291.986563208829, + -292.57229225484906, -293.15919965241676, -293.7472845257627, -294.3365460044106, + -294.9269875259936, -295.51861517154896, -296.1114325809504, -296.70543940721166, + -297.3006351696594, -297.89702307577414, -298.49460872719874, -299.0933957191521, + -299.69338416362336, -300.29457393640496, -300.8969680610969, -301.50057172055426, + -302.10538845895877, -302.71141878594295, -303.31866290235337, -303.92712368726046, + -304.53680595921657, -305.14771320782285, -305.7598462866421, -306.3732056921633, + -306.98779419222967, -307.6036162899247, -308.2206754190174, -308.83897273011434, + -309.4585089879684, -310.0792868772328, -310.70131062915857, -311.3245836222539, + -311.94910726337764, -312.5748825602012, -313.20191213766174, -313.830199994053, + -314.45974945461137, -315.0905621470852, -315.72263929872355, -316.3559834942804, + -316.9905985336233, -317.62648769178344, -318.2636527868513, -318.9020952442645, + -319.5418176247443, -320.18282356031045, -320.8251162796443, -321.4686977648914, + -322.11356962017334, -322.7597343954331, -323.4071955818418, -324.0559563661364, + -324.7060188719847, -325.35738486448656, -326.01005689360136, -326.6640383334178, + -327.3193323334846, -327.97594113975754, -328.63386666231656, -329.29311145984894, + -329.95367881022037, -330.6155718307366, -331.2787928732944, -331.94334397856255, + -332.6092277209164, -333.2764473002516, -333.9450058066361, -334.6149056840792, + -335.286149090949, -335.9587386228065, -336.6326774175029, -337.3079685428293, -337.9846145232684, + -338.6626176233944, -339.3419804642377, -340.022706135458, -340.7047976874095, -341.3882577153029, + -342.07308857971486, -342.75929293043066, -343.4468738209348, -344.1358342888007, + -344.826176991857, -345.5179043776647, -346.2110191272336, -346.90552426826605, + -347.601422829986, -348.29871752613354, -348.99741088331945, -349.6975056155889, + -350.3990047338262, -351.1019112630828, -351.80622796750447, -352.51195744580275, + -353.21910244634364, -353.9276659709034, -354.63765104426665, -355.3490604765017, + -356.0618969323631, -356.7761631954104, -357.4918622649251, -358.2089971690505, + -358.927570760162, -359.64758576380353, -360.36904499927863, -361.09195146903556, + -361.8163082079192, -362.5421181077289, -363.2693839502654, -363.99810859088376, + -364.72829504003204, -365.4599463423245, -366.1930654267157, -366.9276551273741, + -367.6637183358344, -368.4012580746632, -369.14027740104433, -369.88077927933233, + -370.6227665927477, -371.3662422690045, -372.11120934629133, -372.8576708969081, + -373.60562991928094, -374.3550893428728, -375.1060521314913, -375.85852134192436, + -376.6125000638935, -377.3679913289216, -378.12499811035156, -378.88352340794614, + -379.64357029961894, -380.4051418945974, -381.1682412568148, -381.9328714015242, + -382.69903536427796, -383.46673624626146, -384.2359771780845, -385.0067612556417, + -385.7790915344696, -386.5529710857383, -387.32840303572766, -388.1053905381184, + -388.8839367205077, -389.66404467738937, -390.4457175153868, -391.2289583874262, + -392.0137704717747, -392.8001569276271, -393.58812088735715, -394.37766549287124, + -395.1687939249686, -395.96150938755676, -396.7558150704628, -397.55171414010636, + -398.3492097647753, -399.1483051294121, -399.94900339695914, -400.7513076097339, + -401.55522051642305, -402.3607442126537, -403.1678793129748, -403.97662313703535, + -404.78696601570533, -405.5988841676912, -406.41232657422364, -407.22719193743313, + -408.04329038092243, -408.86028352164305, -409.67759674852243, -410.49430019593217, + -411.30896127385165, -412.11948227644524, -412.9229503097521, -413.71553969665734, + -414.49251289191534, -415.2483582419702, -415.9770785993685, -416.6726079072575, + -417.329295131207, -417.94237131855783, -418.5083166266741, -419.02506982614034, + -419.4920624207183, -419.91009801422723, -420.28112301456906, -420.60794297702034, + -420.89393277454684] +z: [8.660695913847082, 66.39862553863676, 127.55801351244567, 180.5416567670594, 210.78937741627567, + 205.98510123498664, 161.75492603580668, 84.86410600259634, -7.938204250916348, -95.46180594158663, + -159.6160710741801, -191.12823315769305, -191.1077383247354, -168.75160790760808, + -136.7976917935674, -106.70965336063549, -85.49283736476575, -74.97102632230975, + -72.98493565768426, -75.44155069558714, -78.2857794410305, -78.76734664689184, -75.79970900381551, + -69.60926545532763, -61.02352026219444, -50.731204460839145, -38.784872157834826, + -24.48063415135472, -6.604904189438815, 16.003062940241094, 43.61116166930451, 74.59937262542272, + 104.69471826704422, 127.10591770220823, 134.08234145821157, 119.62663119956167, + 82.25187144352125, 26.508018246556365, -37.60809657045731, -97.50294343727298, -142.05752447785434, + -165.01189926649766, -166.22428468257075, -150.6580066881632, -125.88440608451583, + -99.27427444087671, -76.01596349902346, -58.51176921704588, -46.89688221845846, + -40.0319509439571, -36.428785825300736, -34.84658879993211, -34.526729879516196, + -35.16461507569286, -36.74598621919215, -39.344955846661044, -42.94609674951552, + -47.33366981479339, -52.08037130315903, -56.64311306447958, -60.522268045019175, + -63.40062822000451, -65.19777275447987, -66.03688501834353, -66.16552673973, -65.87798906520248, + -65.46886057404075, -65.21885341822762, -65.38814742633711, -66.18701058321993, + -67.71310218370519, -69.87490985875417, -72.34304758340024, -74.57596554892733, + -75.9445158717209, -75.92859524663281, -74.30883578638903, -71.26712435663396, -67.34312951723155, + -63.24836193520065, -59.60140045704585, -56.690265423268045, -54.35501112675093, + -52.025856935869335, -48.89272594930154, -44.14172332474001, -37.177555708112656, + -27.766047180552132, -16.071976274482385, -2.617381566487062, 11.77309724198873, + 25.976102073264208, 38.53687298107297, 47.718551492321396, 51.76605423223608, 49.38878979505458, + 40.290429255399275, 25.491354807794263, 7.246430327597227, -11.475853168764028, + -27.772349573580907, -39.515015426681174, -45.713572144488516, -46.46431428981413, + -42.61798316112623, -35.37703866753643, -25.9934677516497, -15.628487382325641, + -5.320192334714672, 4.040136476501501, 11.774699698468378, 17.500102100664304, 21.15959975218581, + 22.97185786614963, 23.31019232970664, 22.558042956454567, 20.99209210295209, 18.729017300170803, + 15.74651164213902, 11.966823265504356, 7.376265857048469, 2.1431266936820315, -3.3126251807763514, + -8.343814311527554, -12.216153231932507, -14.333645421496714, -14.465721023384898, + -12.87582842967554, -10.283561071346963, -7.656458262095206, -5.898235147224569, + -5.548464248913495, -6.607585778672925, -8.55034572367405, -10.517780185318616, + -11.61186892570347, -11.181289206074855, -8.998149061838516, -5.278041737321532, + -0.5603284740797135, 4.485651709130345, 9.246488587633161, 13.28299530174339, 16.36756662020893, + 18.466571978647558, 19.704100343285322, 20.32815531831131, 20.677451138832144, 21.13258932476601, + 22.03831545911599, 23.60334269570648, 25.810407707445663, 28.386041772609907, 30.87468716575818, + 32.83047926730481, 34.087238764628125, 35.01230737770861, 36.621360874602765, 40.45070511179779, + 48.15601919282285, 60.917623297793895, 78.84123893360714, 100.59365670804146, 123.46645783546617, + 143.91789347099578, 158.44750364654655, 164.50260889528977, 161.097954064323, 148.96575759387107, + 130.25873364860564, 107.99215885210386, 85.46056101968273, 65.78797889606493, 51.62771188476158, + 44.92502099696882, 46.66619035735277, 56.63547648764704, 73.29795847361908, 93.94368722527568, + 115.14660187881398, 133.44963208761388, 146.06803902842015, 151.38700573272683, + 149.12923984177337, 140.2151559819424, 126.44367536795221, 110.13915686176617, 93.84830850562516, + 80.0820870481205, 71.04848324787652, 68.34789291609472, 72.6765134206983, 83.63977979446459, + 99.77023650435606, 118.77456269522624, 137.94387073801647, 154.60399035259434, 166.49431469790014, + 172.0327438962138, 170.4928109096748, 162.13218328502208, 148.26316136662118, 131.1890050423139, + 113.90461235170372, 99.51589491572973, 90.46166485727544, 87.7622777984477, 90.58353615476729, + 96.33891906881483, 101.3726007035209, 102.04139409749659, 95.84221536672281, 82.19477689643513, + 62.61034213227201, 40.20548872373059, 18.75852748002458, 1.657957295481827, -8.90300742366735, + -12.266306640477236, -9.163238878856461, -1.233167864960895, 9.564495981683649, + 21.43992022175332, 33.051379335207095, 43.56592729978036, 52.568382283902764, 59.9082904825656, + 65.55991544152265, 69.53869459501789, 71.88526118098649, 72.70284621092118, 72.21843649851635, + 70.83368992994872, 69.13827315439758, 67.87334691224184, 67.8505290321987, 69.8442171675916, + 74.47726967536957, 82.11311705302812, 92.75985141769938, 105.99298201503689, 120.91543676261807, + 136.18772657541675, 150.16300634223884, 161.13869607756567, 167.68973106972928, + 168.99857035076562, 165.07365193284676, 156.7707462316869, 145.59585156800276, 133.34673828809346, + 121.70779304048943, 111.92368383650255, 104.63857613545397, 99.9202392953937, 97.42415696314441, + 96.61636301843436, 96.97307879858319, 98.1025495789197, 99.77349073377508, 101.86901694512392, + 104.30470000982287, 106.95197406963027, 109.5967587731362, 111.94423545513219, 113.66135899930364, + 114.4352894158048, 114.02254875294729, 112.27073376540599, 109.10848671943762, 104.5142402049289, + 98.48427593329139, 91.02225206964965, 82.16482996592228, 72.04340823046537, 60.9644383318998, + 49.47596335726721, 38.38208716267078, 28.675265153703773, 21.380090006656577, 17.336295088765645, + 16.98025827773935, 20.1977232072601, 26.30579134436739, 34.1820237918946, 42.50787279716556, + 50.05364436655743, 55.91974464196498, 59.66868428102189, 61.32427965788826, 61.25993059906409, + 60.02892932141973, 58.19725874007958, 56.22498874652909, 54.41596991142811, 52.9291999133868, + 51.82801400185255, 51.138336589289274, 50.892598938155075, 51.146726044509975, 51.96875114225244, + 53.40564661532219, 55.43870808852528, 57.93823519360839, 60.62739916553108, 63.064899619087015, + 64.65655841637202, 64.7055801259163, 62.50676710339701, 57.47927602641731, 49.31661470793777, + 38.11706927966492, 24.4510077185352, 9.330391041091739, -5.928968195692799, -19.927807358089126, + -31.432334046946572, -39.58564206883777, -44.02742340146397, -44.89862410201391, + -42.74448812517229, -38.35747836190584, -32.61145550528262, -26.32954437918894, + -20.20719977067912, -14.789151242435427, -10.48250380540003, -7.582119063177211, + -6.287826913269372, -6.702621306839947, -8.81253570937736, -12.45851173987955, -17.315467703254903, + -22.89273066518226, -28.563759038476075, -33.62421205979971, -37.369384313159216, + -39.17768128465537, -38.587042278968454, -35.35458955341083, -29.49368865852931, + -21.285110785761752, -11.260045417067115, -0.15386876206445768, 11.167509802528333, + 21.80199162922029, 30.914335137198602, 37.83281681446311, 42.1250528920106, 43.6424129336149, + 42.527536031710504, 39.1838083377227, 34.20896823571095, 28.298619788384883, 22.130902226323823, + 16.250738211119412, 10.978234041445447, 6.366580287176825, 2.2267366046830612, -1.780708650958427, + -6.007319377392964, -10.670867462380551, -15.74701218807577, -20.935394624211764, + -25.71475763924228, -29.473174580935297, -31.674804797246836, -32.01284071766054, + -30.503625724620882, -27.49687795069467, -23.603695718045664, -19.567702223835404, + -16.117560259075034, -13.838347771722535, -13.087536620644972, -13.9646827725496, + -16.328965936199715, -19.84970900489466, -24.07290465216959, -28.48977493682292, + -32.59854442331317, -35.95539956520115, -38.2137508958304, -39.15228104770502, -38.692332219912934, + -36.90456601359803, -34.004068329378526, -30.332692841352, -26.327899158206776, + -22.478863588907455, -19.27298194233058, -17.13832959626637, -16.389291255919126, + -17.18279456348231, -19.4913537172405, -23.09696936219348, -27.60749078959273, -32.494672428897246, + -37.15076766150979, -40.95788530079359, -43.36162493187602, -43.93846520775633, + -42.446152023679346, -38.848786666305784, -33.313331043792914, -26.180492183033934, + -17.918310843908614, -9.069298172978261, -0.20074671809499756, 8.136492955152297, + 15.441031108092213, 21.29036758626846, 25.372508549246056, 27.522718958733332, 27.762289494620244, + 26.32911239456066, 23.684885172768034, 20.483910329661786, 17.495388855951955, 15.484035434486861, + 15.069215691030688, 16.59509470023471, 20.047924906971748, 25.048393934050562, 30.927904484071945, + 36.87324018466698, 42.10253497425529, 46.024752154331864, 48.33939153393884, 49.0517871424882, + 48.40580399084933, 46.76081807159332, 44.45507771693884, 41.69825116535199, 38.52289882103032, + 34.803261444171405, 30.327999320714078, 24.898546537842716, 18.420586405729864, + 10.962638364832946, 2.769145218537321, -5.769675941714455, -14.177570162462116, + -21.96287190097139, -28.682775714701705, -33.98993062535793, -37.65744218731808, + -39.584533342726225, -39.78848208739123, -38.38883996706238, -35.58823507210259, + -31.651659030216656, -26.884305680876444, -21.60752145164577, -16.133301088860883, + -10.739449278450719, -5.649094917111724, -1.018762093669203, 3.061876393936713, + 6.558023261640048, 9.468369818928435, 11.800068844557336, 13.547269650304042, 14.679655966520992, + 15.144569931188034, 14.882211925027542, 13.849398415516339, 12.04474239771272, 9.52770863530006, + 6.425839876890219, 2.9278589763920806, -0.735815798162439, -4.320509611419369, -7.596486438809738, + -10.37389619761122, -12.519116524619744, -13.960610192047202, -14.685038419335728, + -14.726561926961596, -14.153639370214792, -13.057964531219815, -11.549353043113381, + -9.758421987878664, -7.846069969532445, -6.015682302494043, -4.521627491850837, + -3.6670759284008367, -3.78622388922754, -5.210529940531722, -8.224269075041521, + -13.019390902549098, -19.661194514880624, -28.07379403356179, -38.04868886403423, + -49.2733259570762, -61.37184021856482, -73.94857243204568, -86.62633925391154, -99.07453382284537, + -111.02550604149512] +pitch: [-157.8684778032057, -158.3698356698477, -159.00269434786534, -159.7713293302798, + -160.66230277415343, -161.6470704856313, -162.69256942436138, -163.77220996055956, + -164.87013794172887, -165.9791087337121, -167.09653844180943, -168.22171491746042, + -169.35450952912657, -170.49493872451055, -171.6430483690157, -172.79888945081638, + -173.96251395289818, -175.13397428130384, -176.31332320177768, -177.50061383591552, + -178.69589966306725, -179.89923452271375, -181.1106726168898, -182.33026851262574, + -183.55807714440536, -184.7941538166402, -186.0385542061607, -187.29133436472392, + -188.5525507215386, -189.8222600858066, -191.10051964928178, -192.38738698884652, + -193.6829200691045, -194.98717724499227, -196.3002172644072, -197.62209927085402, + -198.95288280610868, -200.29262781290038, -201.64139463761154, -202.99924403299593, + -204.36623716091532, -205.7424355950942, -207.12790132389352, -208.52269675310262, + -209.92688470875024, -211.3405284399345, -212.76369162167163, -214.19643835776446, + -215.6388331836894, -217.09094106950369, -218.55282742277168, -220.02455809151112, + -221.50619936715893, -222.99781798755748, -224.49948113996047, -226.0112564640592, + -227.53321205502968, -229.0654164665993, -230.60793871413497, -232.16084827775205, + -233.72421510544342, -235.2981096162307, -236.88260270333552, -238.47776573737366, + -240.08367056956885, -241.70038953498985, -243.3279954558082, -244.96656164457897, + -246.6161619075422, -248.27687054794822, -249.9487623694034, -251.63191267924097, + -253.32639729191183, -255.03229253240016, -256.7496752396611, -258.4786227700819, + -260.2192130009664, -261.9715243340426, -263.73563569899414, -265.5116265570161, + -267.2995769043935, -269.0995672761047, -270.9116787494495, -272.7359929477005, + -274.57259204377965, -276.42155876396043, -278.28297639159297, -280.15692877085615, + -282.04350031053394, -283.9427759878175, -285.85484135213284, -287.77978252899436, + -289.71768622388356, -291.66863972615556, -293.6327309129702, -295.61004825325045, + -297.6006808116674, -299.60471825265194, -301.62225084443344, -303.65336946310623, + -305.69816559672216, -307.7567313494122, -309.82915944553446, -311.91554323385157, + -314.0159766917348, -316.1305544293973, -318.2593716941557, -320.40252437472, -322.5601090055133, + -324.7322227710197, -326.91896351016163, -329.12042972070725, -331.33672056370636, + -333.56793586795794, -335.81417613450594, -338.0755425411669, -340.3521369470865, + -342.64406189732756, -344.9514206274901, -347.27431706835876, -349.61285585057675, + -351.9671423093153, -354.337282488798, -356.72338314601444, -359.1255517507706, + -361.5438964706455, -363.97852609840754, -366.4295497760087, -368.8970760521407, + -371.3812099206837, -373.88204421775976, -376.39963653729615, -378.9339521671109, + -381.48473467467466, -384.0512382159208, -386.631726774158, -389.2226378464643, + -391.81736161291474, -394.40474356794095, -396.9676796187925, -399.48243974549047, + -401.9194238634856, -404.2457230176494, -406.4291524303254, -408.4426925007373, + -410.267999574875, -411.89703489626, -413.33164749245606, -414.5816483484149, -415.66220963815863] +roll: [-157.8684778032057, -158.3698356698477, -159.00269434786534, -159.7713293302798, + -160.66230277415343, -161.6470704856313, -162.69256942436138, -163.77220996055956, + -164.87013794172887, -165.9791087337121, -167.09653844180943, -168.22171491746042, + -169.35450952912657, -170.49493872451055, -171.6430483690157, -172.79888945081638, + -173.96251395289818, -175.13397428130384, -176.31332320177768, -177.50061383591552, + -178.69589966306725, -179.89923452271375, -181.1106726168898, -182.33026851262574, + -183.55807714440536, -184.7941538166402, -186.0385542061607, -187.29133436472392, + -188.5525507215386, -189.8222600858066, -191.10051964928178, -192.38738698884652, + -193.6829200691045, -194.98717724499227, -196.3002172644072, -197.62209927085402, + -198.95288280610868, -200.29262781290038, -201.64139463761154, -202.99924403299593, + -204.36623716091532, -205.7424355950942, -207.12790132389352, -208.52269675310262, + -209.92688470875024, -211.3405284399345, -212.76369162167163, -214.19643835776446, + -215.6388331836894, -217.09094106950369, -218.55282742277168, -220.02455809151112, + -221.50619936715893, -222.99781798755748, -224.49948113996047, -226.0112564640592, + -227.53321205502968, -229.0654164665993, -230.60793871413497, -232.16084827775205, + -233.72421510544342, -235.2981096162307, -236.88260270333552, -238.47776573737366, + -240.08367056956885, -241.70038953498985, -243.3279954558082, -244.96656164457897, + -246.6161619075422, -248.27687054794822, -249.9487623694034, -251.63191267924097, + -253.32639729191183, -255.03229253240016, -256.7496752396611, -258.4786227700819, + -260.2192130009664, -261.9715243340426, -263.73563569899414, -265.5116265570161, + -267.2995769043935, -269.0995672761047, -270.9116787494495, -272.7359929477005, + -274.57259204377965, -276.42155876396043, -278.28297639159297, -280.15692877085615, + -282.04350031053394, -283.9427759878175, -285.85484135213284, -287.77978252899436, + -289.71768622388356, -291.66863972615556, -293.6327309129702, -295.61004825325045, + -297.6006808116674, -299.60471825265194, -301.62225084443344, -303.65336946310623, + -305.69816559672216, -307.7567313494122, -309.82915944553446, -311.91554323385157, + -314.0159766917348, -316.1305544293973, -318.2593716941557, -320.40252437472, -322.5601090055133, + -324.7322227710197, -326.91896351016163, -329.12042972070725, -331.33672056370636, + -333.56793586795794, -335.81417613450594, -338.0755425411669, -340.3521369470865, + -342.64406189732756, -344.9514206274901, -347.27431706835876, -349.61285585057675, + -351.9671423093153, -354.337282488798, -356.72338314601444, -359.1255517507706, + -361.5438964706455, -363.97852609840754, -366.4295497760087, -368.8970760521407, + -371.3812099206837, -373.88204421775976, -376.39963653729615, -378.9339521671109, + -381.48473467467466, -384.0512382159208, -386.631726774158, -389.2226378464643, + -391.81736161291474, -394.40474356794095, -396.9676796187925, -399.48243974549047, + -401.9194238634856, -404.2457230176494, -406.4291524303254, -408.4426925007373, + -410.267999574875, -411.89703489626, -413.33164749245606, -414.5816483484149, -415.66220963815863] +x: [-127.90269211939827, -131.2678097441197, -136.70543077518892, -142.80201308388703, + -147.68698584712374, -150.43090024970653, -151.93731066347436, -153.97975151042488, + -157.1611591307891, -160.11613483598717, -160.9024782819122, -159.04126159161888, + -156.07990100822514, -154.23554261200883, -154.56584426858808, -156.35317090044515, + -158.04548754985666, -158.68146511423873, -158.50567928984265, -158.41585507282485, + -158.95307629614172, -159.79740407636362, -160.1014847915644, -159.23338066405643, + -157.30653996810443, -155.20316918530557, -154.14843270998995, -155.05581305813232, + -157.9836096719787, -162.09393936591388, -166.22381774921027, -169.63236007909416, + -172.26672230472926, -174.36451975460542, -175.90099493885046, -176.49219286857985, + -175.798054128794, -173.93808909631377, -171.4925114649683, -169.12343773356466, + -167.17664041477232, -165.5708829941525, -164.00639105353508, -162.2887264315374, + -160.51670606493755, -159.0215878949382, -158.1431878867084, -158.0223901064516, + -158.5297306199403, -159.33939301424593, -160.09571223386598, -160.59095964674498, + -160.8477855836447, -161.02451272164438, -161.17795428146462, -161.05434333340307, + -160.10141346846075, -157.75281536914142, -153.82556545048772, -148.76090920354517, + -143.52247341799338, -139.17693928388738, -136.36789818536403, -134.95124204564573, + -133.9874074915166, -132.1254263634103, -128.22234525346727, -121.90425650473114, + -113.78832718004412, -105.27070727944219, -98.02475400195476, -93.46091461275282, + -92.30826635434332, -94.3390728032595, -98.26494379864691, -101.94885284908618, + -103.04849210213153, -99.9163743951314, -92.28166421723154, -81.29521224087058, + -68.95793224897206, -57.37532906713539, -48.286218251495455, -42.9311115531321, + -41.97664928415379, -45.25819014572354, -51.473054204178524, -58.23430997331244, + -62.75013392767508, -62.9223262047309, -58.280188016636316, -50.21077109811404, + -41.35681129895958, -34.4990081709638, -31.449867093567427, -32.42754723012978, + -36.15011519371447, -40.58089839438473, -43.94788650904792, -45.50317129106411, + -45.62757380883436, -45.28654859482272, -45.24609917358149, -45.57109091310874, + -45.68440108090419, -44.8673235962936, -42.817117026428, -39.89366389594422, -36.92267974205595, + -34.701751190785245, -33.51756732176649, -32.957648526197254, -32.118390119251, + -30.08038179415433, -26.373159258276946, -21.169894158050823, -15.12315035159766, + -8.966988036828536, -3.135614678740315, 2.3860480980408405, 7.925467603882083, 13.822714648008436, + 20.175371018999993, 26.7822639200393, 33.293134470121245, 39.42975802508738, 45.09936774714966, + 50.30205732126428, 54.88772677310383, 58.342229552134924, 59.78844212222738, 58.2636935332989, + 53.15779898393836, 44.5830880558185, 33.47443622424592, 21.363593342632985, 9.940566867866028, + 0.6026213709298839, -5.8399344345675575, -9.240708782689756, -10.022124423046609, + -8.998133886903553, -7.186216889606415, -5.63851099666342, -5.297145100741871, -6.879790086756265, + -10.809823525239507, -17.202765649327844, -25.90589109754088, -36.57282515330877] +y: [-95.1053320679519, -102.57942707703143, -115.79349366134936, -132.91150346877552, + -149.8018623891795, -161.15737205071278, -162.80068928833558, -153.8389080682052, + -137.30710232263985, -118.90086421096659, -104.39325930921773, -97.04253564148495, + -96.49753527612377, -99.89130313663716, -104.21834166352956, -108.11727712225031, + -111.8967411258529, -116.28298547066109, -121.23895629376239, -125.73427933608644, + -128.36883519321356, -128.19087124969568, -125.13859527382397, -120.00974008025909, + -114.2430140709469, -109.69248942902398, -108.19817332485465, -110.78624497979699, + -116.93512079376003, -124.6998747734192, -131.86718572831617, -137.27226089357666, + -141.20696974324156, -144.759479700215, -148.87001560457742, -153.88261513985736, + -159.68734476449134, -166.07420632507709, -172.96469725505096, -180.3920700150901, + -188.21937883314945, -195.72926730208002, -201.41736840430042, -203.31624017701841, + -199.82553623123383, -190.59338598173065, -176.88869378565937, -161.2238841338048, + -146.45822831996625, -134.87673586346156, -127.65308031796329, -124.80090469016707, + -125.45619356179108, -128.27549428216608, -131.84101844992867, -135.07530215082386, + -137.65337624491679, -140.25913711418366, -144.43087684329745, -151.8938368423789, + -163.67339640117606, -179.56328628213708, -198.32489175456183, -218.42489246839165, + -238.7164724172193, -258.60743976458997, -277.7429960783446, -295.56813001110913, + -311.0788097160963, -322.80675460113724, -328.93908316116415, -327.56037608593266, + -317.12330108413994, -297.14900905724227, -268.84769440086353, -235.16971112565426, + -200.04553112593794, -167.12032560737575, -138.66238790035453, -115.19890389580594, + -95.95279511481895, -79.72820892106228, -65.76755588438334, -54.22790801461051, + -46.13190044244114, -42.84004879066222, -45.24539383402826, -52.99497625151277, + -64.07018690090207, -74.97804538426473, -81.59594832574722, -80.41038856548796, + -69.63949990787273, -49.72242741663517, -22.966995663147852, 7.39427794510237, 38.19309913101399, + 66.89173432062842, 91.64289937473961, 111.13226208974095, 124.5518791391896, 131.77418236143038, + 133.46586276149228, 130.83435645169016, 124.99732180672979, 116.333135612117, 104.26680906118598, + 87.68178840678189, 65.74204569072383, 38.662165827940036, 8.014089907704136, -23.571276834584964, + -53.142563231420375, -78.2522773792973, -97.51345531907658, -110.70725605031265, + -118.5016237829998, -122.00842291412158, -122.404338258332, -120.6995916626668, + -117.59445385917022, -113.3407631971568, -107.62417911491204, -99.58313488352168, + -88.06247336126238, -72.06061544362858, -51.18535335279474, -25.910478578073977, + 2.4559772454057507, 32.01779445834423, 60.591762572708525, 85.94477375005923, 105.9576058260377, + 118.78964738570927, 123.11631379953998, 118.43389190043557, 105.33379726489348, + 85.60032102947753, 62.014359671431286, 37.84509121681435, 16.144470009647492, -0.9347463893425387, + -12.559784399157243, -19.23707231899498, -22.45859920077777, -24.150622679079273, + -26.143960951682608, -29.820707661306084, -35.98068482695456, -44.88212815398885] +yaw: [-157.8684778032057, -158.3698356698477, -159.00269434786534, -159.7713293302798, + -160.66230277415343, -161.6470704856313, -162.69256942436138, -163.77220996055956, + -164.87013794172887, -165.9791087337121, -167.09653844180943, -168.22171491746042, + -169.35450952912657, -170.49493872451055, -171.6430483690157, -172.79888945081638, + -173.96251395289818, -175.13397428130384, -176.31332320177768, -177.50061383591552, + -178.69589966306725, -179.89923452271375, -181.1106726168898, -182.33026851262574, + -183.55807714440536, -184.7941538166402, -186.0385542061607, -187.29133436472392, + -188.5525507215386, -189.8222600858066, -191.10051964928178, -192.38738698884652, + -193.6829200691045, -194.98717724499227, -196.3002172644072, -197.62209927085402, + -198.95288280610868, -200.29262781290038, -201.64139463761154, -202.99924403299593, + -204.36623716091532, -205.7424355950942, -207.12790132389352, -208.52269675310262, + -209.92688470875024, -211.3405284399345, -212.76369162167163, -214.19643835776446, + -215.6388331836894, -217.09094106950369, -218.55282742277168, -220.02455809151112, + -221.50619936715893, -222.99781798755748, -224.49948113996047, -226.0112564640592, + -227.53321205502968, -229.0654164665993, -230.60793871413497, -232.16084827775205, + -233.72421510544342, -235.2981096162307, -236.88260270333552, -238.47776573737366, + -240.08367056956885, -241.70038953498985, -243.3279954558082, -244.96656164457897, + -246.6161619075422, -248.27687054794822, -249.9487623694034, -251.63191267924097, + -253.32639729191183, -255.03229253240016, -256.7496752396611, -258.4786227700819, + -260.2192130009664, -261.9715243340426, -263.73563569899414, -265.5116265570161, + -267.2995769043935, -269.0995672761047, -270.9116787494495, -272.7359929477005, + -274.57259204377965, -276.42155876396043, -278.28297639159297, -280.15692877085615, + -282.04350031053394, -283.9427759878175, -285.85484135213284, -287.77978252899436, + -289.71768622388356, -291.66863972615556, -293.6327309129702, -295.61004825325045, + -297.6006808116674, -299.60471825265194, -301.62225084443344, -303.65336946310623, + -305.69816559672216, -307.7567313494122, -309.82915944553446, -311.91554323385157, + -314.0159766917348, -316.1305544293973, -318.2593716941557, -320.40252437472, -322.5601090055133, + -324.7322227710197, -326.91896351016163, -329.12042972070725, -331.33672056370636, + -333.56793586795794, -335.81417613450594, -338.0755425411669, -340.3521369470865, + -342.64406189732756, -344.9514206274901, -347.27431706835876, -349.61285585057675, + -351.9671423093153, -354.337282488798, -356.72338314601444, -359.1255517507706, + -361.5438964706455, -363.97852609840754, -366.4295497760087, -368.8970760521407, + -371.3812099206837, -373.88204421775976, -376.39963653729615, -378.9339521671109, + -381.48473467467466, -384.0512382159208, -386.631726774158, -389.2226378464643, + -391.81736161291474, -394.40474356794095, -396.9676796187925, -399.48243974549047, + -401.9194238634856, -404.2457230176494, -406.4291524303254, -408.4426925007373, + -410.267999574875, -411.89703489626, -413.33164749245606, -414.5816483484149, -415.66220963815863] +z: [27.487690876753174, 15.337246293823744, -6.519501713577381, -32.17378390020878, + -52.95176670190418, -61.55800766961198, -55.94481745228971, -40.02071700799717, + -21.554360535385083, -9.07142702001839, -8.346860847184745, -19.368396082981224, + -36.19083663458397, -50.83073806660286, -58.44885149238343, -59.53297302408236, + -57.86810718901493, -56.968343215030046, -58.092142359329614, -60.51600905693699, + -62.7444480158786, -63.35926464089802, -61.25799874825209, -55.70616111817719, -46.57263622352219, + -34.71402685096095, -22.12817933348742, -11.450779533332875, -4.771195360275572, + -2.4341668275669868, -2.815083023486069, -3.436788885507724, -2.639407740293576, + -0.49257916688823505, 1.6634057300540754, 2.4787157173774994, 1.5816568083905458, + -0.3081254457303777, -1.9465496236895794, -2.235946638392112, -0.587599350222585, + 3.0830448889107243, 8.585148043725034, 15.741871127579394, 24.588070885472288, 35.317967966866895, + 47.92047635319786, 61.70329222579224, 75.14570192551099, 86.40468533274036, 94.26512130476762, + 98.82097106678889, 101.29642767567256, 103.1652314081855, 105.33907861244916, 107.99131029859556, + 110.83900010952557, 113.33429248514499, 114.58664344276714, 113.36413234926317, + 108.50758731712286, 99.61208997647628, 87.50344465477376, 74.1768095655181, 62.22532988939497, + 54.03291376712356, 51.07089312083894, 53.5800909155043, 60.73095402033792, 71.08141473731635, + 83.00300927492155, 94.88248494409528, 105.19436856609353, 112.66234904016706, 116.5532902732588, + 116.89064945934301, 114.34290812223043, 109.7932211178687, 103.88036334169092, 96.83894755858785, + 88.72518958612778, 79.81089200833654, 70.81116373157523, 62.753896533262505, 56.569823287756314, + 52.671108932076606, 50.767905449313446, 49.98887184490788, 49.18533829409316, 47.24706022687302, + 43.33944398884515, 37.069957898660974, 28.60206393388892, 18.673743747050306, 8.448948216774712, + -0.7972607166725888, -8.036081276408424, -12.709833208187701, -14.735329114490181, + -14.3542414036342, -11.989651298267246, -8.214474065551915, -3.796221322329577, + 0.3127831108565613, 3.1349675066051397, 3.941404446156438, 2.4944463514526865, -0.8862482050049294, + -5.474872423279538, -10.422540767592642, -15.02656078493345, -18.872907062153832, + -21.8268990816907, -23.925876718830928, -25.25373825675896, -25.85191903924253, + -25.679848344814015, -24.61517234703336, -22.486711991951278, -19.140322650202826, + -14.528471985404261, -8.792284878821462, -2.2948582536620714, 4.415692468816276, + 10.697174253055142, 15.917112597767035, 19.536456023535326, 21.170365945226813, + 20.644086856810723, 18.052446724627877, 13.800892715061188, 8.584501211461651, 3.2736044258695483, + -1.2815934000814575, -4.466744501254597, -6.057544821897366, -6.251111895303721, + -5.565299343444047, -4.652409384326363, -4.10510631771834, -4.32579622671807, -5.493001584407323, + -7.61115583178264, -10.59834849765868, -14.363422130581966, -18.8448260745628, -24.011142744024184, + -29.84051753345265, -36.298155831550744, -43.32322171459229] diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/package.xml b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/package.xml index b647a5d85..a3572177f 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/package.xml +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/package.xml @@ -25,7 +25,7 @@ rosplan_dispatch_msgs rosplan_knowledge_msgs diagnostic_msgs - mcr_perception_msgs + mas_perception_msgs mdr_rosplan_interface mdr_move_arm_action mdr_move_base_action @@ -43,7 +43,7 @@ rosplan_dispatch_msgs rosplan_knowledge_msgs diagnostic_msgs - mcr_perception_msgs + mas_perception_msgs mdr_rosplan_interface mdr_move_arm_action mdr_move_base_action diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/launch/place_client.launch b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/launch/place_client.launch index 098b273b3..610a4555c 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/launch/place_client.launch +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/launch/place_client.launch @@ -2,8 +2,6 @@ - - @@ -11,8 +9,6 @@ - - diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_action b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_action index 0c14a80b1..38d267601 100755 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_action +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_action @@ -1,22 +1,38 @@ #!/usr/bin/env python import rospy -import smach - -from smach_ros import ActionServerWrapper, IntrospectionServer +import actionlib from mdr_place_action.msg import PlaceAction -from mdr_place_action.action_states import (SetupPlace, Place, SetActionLibResult) +from mdr_place_action.action_states import PlaceSM +class PlaceServer(object): + '''A server exposing a placing action. -class PlaceSkill(smach.StateMachine): - def __init__(self): - smach.StateMachine.__init__(self, - outcomes=['OVERALL_SUCCESS', - 'OVERALL_FAILED', 'PREEMPTED'], - input_keys=['place_goal'], - output_keys=['place_feedback', - 'place_result']) + The server expects the following parameters to be made available on the ROS parameter server: + * move_arm_server: Name of the move_arm action server (default: 'move_arm_server') + * move_base_server: Name of the move_base action server (default: 'move_base_server') + * gripper_controller_pkg_name: The name of a package that implements functionalities + for controlling a robot's gripper (default: 'mdr_gripper_controller') + * preplace_config_name: Name of the preplace configuration (default: 'pregrasp') + * safe_arm_joint_config: Name of a configuration in which the robot can + safely move around the environment (default: 'folded') + * base_elbow_offset: An optional offset between base_link and the manipulator's elbow; + used for aligning the base with the placing pose so that + the manipulator can easily reach it (default: -1) + * placing_dmp: Path to a YAML file containing the weights of a dynamic + motion primitive used for placing (default: '') + * dmp_tau: The value of the temporal dynamic motion primitive parameter (default: 1) + * placing_orientation: For more constrained manipulators, it might make sense to use + a fixed placing orientation (expressed as an (x, y, z, w) quaternion) + to ensure easier reachability; for instance, we might want to keep + the orientation with which an object was grasped instead of allowing + arbitrary orientations (default: [], in which case the argument is ignored) + + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de + ''' + def __init__(self): move_arm_server = rospy.get_param('~move_arm_server', 'move_arm_server') move_base_server = rospy.get_param('~move_base_server', 'move_base_server') gripper_controller_pkg_name = rospy.get_param('~gripper_controller_pkg_name', @@ -28,54 +44,39 @@ class PlaceSkill(smach.StateMachine): placing_dmp = rospy.get_param('~placing_dmp', '') dmp_tau = float(rospy.get_param('~dmp_tau', 1.)) - with self: - smach.StateMachine.add('SETUP_PLACE', SetupPlace(), - transitions={'succeeded': 'PLACE', - 'failed': 'SETUP_PLACE'}) - - smach.StateMachine.add('PLACE', Place(move_arm_server=move_arm_server, - move_base_server=move_base_server, - gripper_controller_pkg_name=gripper_controller_pkg_name, - preplace_config_name=preplace_config_name, - safe_arm_joint_config=safe_arm_joint_config, - base_elbow_offset=base_elbow_offset, - placing_orientation=placing_orientation, - placing_dmp=placing_dmp, - dmp_tau=dmp_tau), - transitions={'succeeded': 'SET_ACTION_LIB_SUCCESS', - 'failed': 'SET_ACTION_LIB_FAILED'}) + rospy.loginfo('[place] Initialising state machine') + self.action_sm = PlaceSM(move_arm_server=move_arm_server, + move_base_server=move_base_server, + gripper_controller_pkg_name=gripper_controller_pkg_name, + preplace_config_name=preplace_config_name, + safe_arm_joint_config=safe_arm_joint_config, + base_elbow_offset=base_elbow_offset, + placing_orientation=placing_orientation, + placing_dmp=placing_dmp, + dmp_tau=dmp_tau) + rospy.loginfo('[place] State machine initialised') - smach.StateMachine.add('SET_ACTION_LIB_FAILED', - SetActionLibResult(False), - transitions={'succeeded': 'OVERALL_FAILED'}) - - smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', - SetActionLibResult(True), - transitions={'succeeded': 'OVERALL_SUCCESS'}) + self.action_server = actionlib.SimpleActionServer('place_server', + PlaceAction, self.execute, False) + self.action_server.start() + rospy.loginfo('place action server ready') + def execute(self, goal): + rospy.loginfo('[place] Received an action request') + self.action_sm.goal = goal + self.action_sm.result = None + self.action_sm.execution_requested = True + while not self.action_sm.result: + rospy.sleep(0.05) + self.action_server.set_succeeded(self.action_sm.result) if __name__ == '__main__': rospy.init_node('place_server') - - # construct state machine - sm = PlaceSkill() - - # smach viewer - sis = IntrospectionServer('place_smach_viewer', sm, - '/PLACE_SMACH_VIEWER') - sis.start() - - asw = ActionServerWrapper( - server_name='place_server', - action_spec=PlaceAction, - wrapped_container=sm, - succeeded_outcomes=['OVERALL_SUCCESS'], - aborted_outcomes=['OVERALL_FAILED'], - preempted_outcomes=['PREEMPTED'], - goal_key='place_goal', - feedback_key='place_feedback', - result_key='place_result') - - # Run the server in a background thread - asw.run_server() - rospy.spin() + place_server = PlaceServer() + try: + place_server.action_sm.run() + while place_server.action_sm.is_running and not rospy.is_shutdown(): + rospy.spin() + except (KeyboardInterrupt, SystemExit): + print('{0} interrupted; exiting...'.format(place_server.action_sm.name)) + place_server.action_sm.stop() diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_client b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_client index 2f311dbf0..6c495d3bd 100755 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_client +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/place_client @@ -10,7 +10,7 @@ from geometry_msgs.msg import Point, Quaternion, Pose, PoseStamped, PointStamped import rosplan_dispatch_msgs.msg as plan_dispatch_msgs import rosplan_knowledge_msgs.srv as rosplan_srvs import diagnostic_msgs.msg as diag_msgs -from mcr_perception_msgs.msg import Object, Plane +from mas_perception_msgs.msg import Object, Plane from action_execution.action import Action from action_execution.utils.configuration import Configuration @@ -20,6 +20,12 @@ from mdr_rosplan_interface.action_client_base import ActionClientBase from mdr_place_action.msg import PlaceAction, PlaceGoal class PlaceClient(ActionClientBase): + '''Defines a client for an object placing action. + + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de + + ''' def __init__(self): super(PlaceClient, self).__init__() @@ -36,7 +42,7 @@ class PlaceClient(ActionClientBase): if self.action_name != msg.name.lower(): return - rospy.loginfo('[PLACE] Action request received') + rospy.loginfo('[place] Action request received') self.action_id = msg.action_id client = actionlib.SimpleActionClient(self.action_server_name, PlaceAction) @@ -44,13 +50,13 @@ class PlaceClient(ActionClientBase): goal = self.get_action_message(msg) # calling the actionlib server and waiting for the execution to end - rospy.loginfo('[PLACE] Sending actionlib goal to %s' % self.action_server_name) + rospy.loginfo('[place] Sending actionlib goal to %s' % self.action_server_name) client.send_goal(goal) client.wait_for_result(rospy.Duration.from_sec(int(self.action_timeout))) result = client.get_result() if result and result.success: - rospy.loginfo('[PLACE] Updating the knowledge base') + rospy.loginfo('[place] Updating the knowledge base') self.update_knowledge_base(goal.pose) self.send_action_feedback(True) else: @@ -75,18 +81,18 @@ class PlaceClient(ActionClientBase): obj_config = Configuration() obj_config.frame_id = self.frame_id - surface_object_names = self.get_surface_object_names(self.obj_plane) - surface_objects = self.get_surface_object(surface_object_names) + surface_object_names = self.kb_interface.get_surface_object_names(self.obj_plane) + surface_objects = self.kb_interface.get_objects(surface_object_names, Object._type) for obj in surface_objects: obj.pose.header.stamp = rospy.Time(0) obj.pose = self.tf_listener.transformPose(self.frame_id, obj.pose) obj_config.static_objs.append(ConverterFactory.convert_ros_msg(obj)) - manipulated_obj = self.get_object_by_name(self.obj) + manipulated_obj = self.kb_interface.get_obj_instance(self.obj, Object._type) manipulated_obj.pose.header.frame_id = self.frame_id obj_config.manipulated_obj = ConverterFactory.convert_ros_msg(manipulated_obj) - surface_plane = self.get_surface_by_name(self.obj_plane) + surface_plane = self.kb_interface.get_obj_instance(self.obj_plane, Plane._type) # we convert the surface center point to the desired frame surface_plane_point = PointStamped() @@ -142,103 +148,22 @@ class PlaceClient(ActionClientBase): candidate_pose = ConverterFactory.convert_to_ros_msg(candidate_pose) return candidate_pose - def get_surface_object_names(self, surface_name): - surface_objects = list() - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = 'on' - result = self.attribute_fetching_client(request) - for item in result.attributes: - object_on_desired_surface = False - object_name = '' - if not item.is_negative: - for param in item.values: - if param.key == 'plane' and param.value == surface_name: - object_on_desired_surface = True - elif param.key == 'obj': - object_name = param.value - if object_on_desired_surface: - surface_objects.append(object_name) - return surface_objects - - def get_surface_object(self, surface_objects): - objects = list() - for obj_name in surface_objects: - obj = self.get_object_by_name(obj_name) - if obj is not None: - objects.append(obj) - return objects - - def get_object_by_name(self, obj_name): - try: - obj = self.msg_store_client.query_named(obj_name, Object._type)[0] - return obj - except: - rospy.logerr('Error retriving knowledge about %s', obj_name) - return None - - def get_surface_by_name(self, surface_name): - try: - surface = self.msg_store_client.query_named(surface_name, Plane._type)[0] - return surface - except: - rospy.logerr('Error retriving knowledge about %s', surface_name) - return None - def update_knowledge_base(self, new_object_pose): - # we remove the fact that the robot is holding the object from the knowledge base - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 2 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'holding' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - request.knowledge.values.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'obj' - arg_msg.value = self.obj - request.knowledge.values.append(arg_msg) - - self.knowledge_update_client(request) - - # we add the fact that the robot's gripper is empty to the knowledge base - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 0 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'empty_gripper' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - request.knowledge.values.append(arg_msg) - - self.knowledge_update_client(request) - - # we add the fact that the object is on the surface to the knowledge base - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 0 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'on' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'obj' - arg_msg.value = self.obj - request.knowledge.values.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'plane' - arg_msg.value = self.obj_plane - request.knowledge.values.append(arg_msg) - - self.knowledge_update_client(request) + '''Updates the knowledge base with the following facts: + * the object is on the placing surface + * the robot's gripper is empty + * the robot is not holding the object anymore + ''' + facts_to_add = [('on', [('obj', self.obj), ('plane', self.obj_plane)]), + ('empty_gripper', [('bot', self.robot_name)])] + facts_to_remove = [('holding', [('bot', self.robot_name), ('obj', self.obj)])] + self.kb_interface.update_kb(facts_to_add, facts_to_remove) # we update the location of the object - placed_object = self.get_object_by_name(self.obj) + placed_object = self.kb_interface.get_obj_instance(self.obj, Object._type) new_object_pose.header.stamp = rospy.Time.now() placed_object.pose = new_object_pose - self.msg_store_client.update_named(self.obj, placed_object) + self.kb_interface.update_obj_instance(self.obj, placed_object) if __name__ == '__main__': rospy.init_node('place_client') diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/throw_client b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/throw_client index 61becdbf5..30065cbc6 100755 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/throw_client +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/scripts/throw_client @@ -8,9 +8,7 @@ from std_msgs.msg import Header from geometry_msgs.msg import Point, Quaternion, Pose, PoseStamped import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import rosplan_knowledge_msgs.srv as rosplan_srvs -import diagnostic_msgs.msg as diag_msgs -from mcr_perception_msgs.msg import Object +from mas_perception_msgs.msg import Object from action_execution.action import Action from ae_ros_message_converter.converter_factory import ConverterFactory @@ -19,6 +17,12 @@ from mdr_rosplan_interface.action_client_base import ActionClientBase from mdr_place_action.msg import PlaceAction, PlaceGoal class ThrowClient(ActionClientBase): + '''Defines a client for an object throwing action. + + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de + + ''' def __init__(self): super(ThrowClient, self).__init__() @@ -35,7 +39,7 @@ class ThrowClient(ActionClientBase): if self.action_name != msg.name.lower(): return - rospy.loginfo('[THROW] Action request received') + rospy.loginfo('[throw] Action request received') self.action_id = msg.action_id client = actionlib.SimpleActionClient(self.action_server_name, PlaceAction) @@ -43,13 +47,13 @@ class ThrowClient(ActionClientBase): goal = self.get_action_message(msg) # calling the actionlib server and waiting for the execution to end - rospy.loginfo('[THROW] Sending actionlib goal to %s' % self.action_server_name) + rospy.loginfo('[throw] Sending actionlib goal to %s' % self.action_server_name) client.send_goal(goal) client.wait_for_result(rospy.Duration.from_sec(int(self.action_timeout))) result = client.get_result() if result and result.success: - rospy.loginfo('[THROW] Updating the knowledge base') + rospy.loginfo('[throw] Updating the knowledge base') self.update_knowledge_base(goal.pose) self.send_action_feedback(True) else: @@ -71,12 +75,12 @@ class ThrowClient(ActionClientBase): return goal def get_throwing_pose(self): - throwing_target = self.get_object_by_name(self.throwing_target) + throwing_target = self.kb_interface.get_obj_instance(self.throwing_target, Object._type) throwing_target.pose.header.stamp = rospy.Time(0) throwing_target.pose = self.tf_listener.transformPose(self.frame_id, throwing_target.pose) throwing_target_obj = ConverterFactory.convert_ros_msg(throwing_target) - obj_to_throw = self.get_object_by_name(self.obj) + obj_to_throw = self.kb_interface.get_obj_instance(self.obj, Object._type) obj_to_throw.pose.header.frame_id = self.frame_id manipulated_obj = ConverterFactory.convert_ros_msg(obj_to_throw) @@ -96,70 +100,23 @@ class ThrowClient(ActionClientBase): candidate_pose = ConverterFactory.convert_to_ros_msg(candidate_pose) return candidate_pose - def get_object_by_name(self, obj_name): - try: - obj = self.msg_store_client.query_named(obj_name, Object._type)[0] - return obj - except: - rospy.logerr('Error retriving knowledge about %s', obj_name) - return None - def update_knowledge_base(self, new_object_pose): - # we remove the fact that the robot is holding the object from the knowledge base - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 2 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'holding' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - request.knowledge.values.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'obj' - arg_msg.value = self.obj - request.knowledge.values.append(arg_msg) - - self.knowledge_update_client(request) - - # we add the fact that the robot's gripper is empty to the knowledge base - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 0 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'empty_gripper' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - request.knowledge.values.append(arg_msg) - - self.knowledge_update_client(request) - - # we add the fact that the object is on the surface to the knowledge base - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 0 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'in' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'obj' - arg_msg.value = self.obj - request.knowledge.values.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'storing_obj' - arg_msg.value = self.throwing_target - request.knowledge.values.append(arg_msg) - - self.knowledge_update_client(request) + '''Updates the knowledge base with the following facts: + * the object is in the throwing target + * the robot's gripper is empty + * the robot is not holding the object anymore + ''' + facts_to_add = [('in', [('obj', self.obj), ('storing_obj', self.throwing_target)]), + ('empty_gripper', [('bot', self.robot_name)])] + facts_to_remove = [('holding', [('bot', self.robot_name), ('obj', self.obj)])] + self.kb_interface.update_kb(facts_to_add, facts_to_remove) # we update the location of the object - thrown_object = self.get_object_by_name(self.obj) + thrown_object = self.kb_interface.get_obj_instance(self.obj, Object._type) new_object_pose.header.stamp = rospy.Time.now() new_object_pose.pose.position.z = 0. thrown_object.pose = new_object_pose - self.msg_store_client.update_named(self.obj, thrown_object) + self.kb_interface.update_obj_instance(self.obj, thrown_object) if __name__ == '__main__': rospy.init_node('throw_client') diff --git a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py index eff8c71bc..f982f5b14 100644 --- a/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_manipulation_actions/mdr_place_action/ros/src/mdr_place_action/action_states.py @@ -2,33 +2,17 @@ from importlib import import_module import rospy -import smach import tf import actionlib from geometry_msgs.msg import PoseStamped +from pyftsm.ftsm import FTSMTransitions +from mas_execution.action_sm_base import ActionSMBase from mdr_move_base_action.msg import MoveBaseAction, MoveBaseGoal from mdr_move_arm_action.msg import MoveArmAction, MoveArmGoal from mdr_place_action.msg import PlaceGoal, PlaceFeedback, PlaceResult -class SetupPlace(smach.State): - def __init__(self): - smach.State.__init__(self, outcomes=['succeeded', 'failed'], - input_keys=['place_goal'], - output_keys=['place_feedback', 'place_result']) - - def execute(self, userdata): - # consider moving the other components of the robot out of the arm's way, though - # this could be dangerous if the robot is for example close to some furniture item - - feedback = PlaceFeedback() - feedback.current_state = 'SETUP_PLACE' - feedback.message = '[SETUP_PLACE] setting up the arm' - userdata.place_feedback = feedback - - return 'succeeded' - -class Place(smach.State): +class PlaceSM(ActionSMBase): def __init__(self, timeout=120.0, gripper_controller_pkg_name='mdr_gripper_controller', preplace_config_name='pregrasp', @@ -38,10 +22,9 @@ def __init__(self, timeout=120.0, base_elbow_offset=-1., placing_orientation=list(), placing_dmp='', - dmp_tau=1.): - smach.State.__init__(self, input_keys=['place_goal'], - output_keys=['place_feedback'], - outcomes=['succeeded', 'failed']) + dmp_tau=1., + max_recovery_attempts=1): + super(PlaceSM, self).__init__('Place', [], max_recovery_attempts) self.timeout = timeout gripper_controller_module_name = '{0}.gripper_controller'.format(gripper_controller_pkg_name) @@ -60,19 +43,28 @@ def __init__(self, timeout=120.0, self.tf_listener = tf.TransformListener() - self.move_arm_client = actionlib.SimpleActionClient(self.move_arm_server, MoveArmAction) - self.move_arm_client.wait_for_server() + self.move_arm_client = None + self.move_base_client = None - self.move_base_client = actionlib.SimpleActionClient(self.move_base_server, MoveBaseAction) - self.move_base_client.wait_for_server() + def init(self): + try: + self.move_arm_client = actionlib.SimpleActionClient(self.move_arm_server, MoveArmAction) + rospy.loginfo('[place] Waiting for %s server', self.move_arm_server) + self.move_arm_client.wait_for_server() + except: + rospy.logerr('[place] %s server does not seem to respond', self.move_arm_server) - def execute(self, userdata): - feedback = PlaceFeedback() - feedback.current_state = 'PLACE' - feedback.message = '[PLACE] moving the arm' - userdata.place_feedback = feedback + try: + self.move_base_client = actionlib.SimpleActionClient(self.move_base_server, MoveBaseAction) + rospy.loginfo('[place] Waiting for %s server', self.move_base_server) + self.move_base_client.wait_for_server() + except: + rospy.logerr('[place] %s server does not seem to respond', self.move_base_server) - pose = userdata.place_goal.pose + return FTSMTransitions.INITIALISED + + def running(self): + pose = self.goal.pose pose.header.stamp = rospy.Time(0) pose_base_link = self.tf_listener.transformPose('base_link', pose) if self.placing_orientation: @@ -82,32 +74,33 @@ def execute(self, userdata): pose_base_link.pose.orientation.w = self.placing_orientation[3] if self.base_elbow_offset > 0: - self.align_base_with_pose(pose_base_link) + self.__align_base_with_pose(pose_base_link) # the base is now correctly aligned with the pose, so we set the # y position of the goal pose to the elbow offset pose_base_link.pose.position.y = self.base_elbow_offset - rospy.loginfo('[PLACE] Moving to a preplace configuration...') - self.move_arm(MoveArmGoal.NAMED_TARGET, self.preplace_config_name) + rospy.loginfo('[place] Moving to a preplace configuration...') + self.__move_arm(MoveArmGoal.NAMED_TARGET, self.preplace_config_name) # we set up the arm group for moving - rospy.loginfo('[PLACE] Placing...') - success = self.move_arm(MoveArmGoal.END_EFFECTOR_POSE, pose_base_link) + rospy.loginfo('[place] Placing...') + success = self.__move_arm(MoveArmGoal.END_EFFECTOR_POSE, pose_base_link) if not success: - rospy.logerr('[PLACE] Arm motion unsuccessful') - return 'failed' + rospy.logerr('[place] Arm motion unsuccessful') + self.result = self.set_result(False) + return FTSMTransitions.DONE - rospy.loginfo('[PLACE] Arm motion successful') - rospy.loginfo('[PLACE] Opening the gripper...') + rospy.loginfo('[place] Arm motion successful') + rospy.loginfo('[place] Opening the gripper...') self.gripper.open() - rospy.loginfo('[PLACE] Moving the arm back') - self.move_arm(MoveArmGoal.NAMED_TARGET, self.safe_arm_joint_config) - - return 'succeeded' + rospy.loginfo('[place] Moving the arm back') + self.__move_arm(MoveArmGoal.NAMED_TARGET, self.safe_arm_joint_config) + self.result = self.set_result(True) + return FTSMTransitions.DONE - def align_base_with_pose(self, pose_base_link): + def __align_base_with_pose(self, pose_base_link): '''Moves the base so that the elbow is aligned with the goal pose. Keyword arguments: @@ -133,7 +126,7 @@ def align_base_with_pose(self, pose_base_link): self.move_base_client.wait_for_result() self.move_base_client.get_result() - def move_arm(self, goal_type, goal): + def __move_arm(self, goal_type, goal): '''Sends a request to the 'move_arm' action server and waits for the results of the action execution. @@ -156,15 +149,7 @@ def move_arm(self, goal_type, goal): result = self.move_arm_client.get_result() return result -class SetActionLibResult(smach.State): - def __init__(self, result): - smach.State.__init__(self, outcomes=['succeeded'], - input_keys=['place_goal'], - output_keys=['place_feedback', 'place_result']) - self.result = result - - def execute(self, userdata): + def set_result(self, success): result = PlaceResult() - result.success = self.result - userdata.place_result = result - return 'succeeded' + result.success = success + return result diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/CMakeLists.txt b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/CMakeLists.txt index 44f7395f8..9a7badf74 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/CMakeLists.txt +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/CMakeLists.txt @@ -9,7 +9,7 @@ find_package(catkin REQUIRED COMPONENTS roslint rospy std_msgs - mcr_door_status + mdr_door_status mdr_move_forward_action ) diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/README.md b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/README.md index a7e712ad3..eeb777007 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/README.md +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/README.md @@ -20,17 +20,17 @@ The action takes no inputs. ## Dependencies * ``std_msgs`` -* ``mcr_door_status`` +* ``mdr_door_status`` * ``mdr_move_forward_action`` ## Example usage -1. Run the action server and its dependencies (namely ``mcr_door_status`` and ``mdr_move_forward_action``): ``roslaunch mdr_enter_door_action enter_door.launch`` +1. Run the action server and its dependencies (namely ``mdr_door_status`` and ``mdr_move_forward_action``): ``roslaunch mdr_enter_door_action enter_door.launch`` 2. Run the client example: ``rosrun mdr_enter_door_action enter_door_action_client_test`` ## Example usage (manual) -1. Launch the node that publishes the status of a door: ``roslaunch mcr_door_status door_status.launch`` +1. Launch the node that publishes the status of a door: ``roslaunch mdr_door_status door_status.launch`` 2. Run the move forward action server: ``roslaunch mdr_move_forward_action move_forward.launch`` 3. Run the action server: ``roslaunch mdr_enter_door_action enter_door_server.launch`` 4. Run the client example: ``rosrun mdr_enter_door_action enter_door_action_client_test`` @@ -40,4 +40,4 @@ The action takes no inputs. 1. Run the move forward action server: ``roslaunch mdr_move_forward_action move_forward.launch`` 2. Run the action server: ``roslaunch mdr_enter_door_action enter_door_server.launch`` 3. Run the client example: ``rosrun mdr_enter_door_action enter_door_action_client_test`` -4. Publish an ``open`` door status: ``rostopic pub -r 1 /mcr_perception/door_status/door_status std_msgs/Bool "data: true"`` +4. Publish an ``open`` door status: ``rostopic pub -r 1 /mdr_perception/door_status/door_status std_msgs/Bool "data: true"`` diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/package.xml b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/package.xml index c3451dfa4..d6d67693b 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/package.xml +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/package.xml @@ -18,7 +18,7 @@ roslint rospy std_msgs - mcr_door_status + mdr_door_status mdr_move_forward_action std_msgs @@ -28,7 +28,7 @@ actionlib_msgs message_runtime std_msgs - mcr_door_status + mdr_door_status mdr_move_forward_action std_msgs diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/launch/enter_door.launch b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/launch/enter_door.launch index fdd17f435..ae3f9f7e3 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/launch/enter_door.launch +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/launch/enter_door.launch @@ -1,5 +1,5 @@ - + diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/scripts/enter_door_action b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/scripts/enter_door_action index 2ca7a3525..e61df452e 100755 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/scripts/enter_door_action +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/scripts/enter_door_action @@ -1,65 +1,55 @@ #!/usr/bin/env python import rospy -import smach - -from smach_ros import ActionServerWrapper, IntrospectionServer +import actionlib from mdr_enter_door_action.msg import EnterDoorAction -from mdr_enter_door_action.action_states import SetupEnterDoor, WaitForDoor, EnterDoor, SetActionLibResult +from mdr_enter_door_action.action_states import EnterDoorSM + +class EnterDoorServer(object): + '''A server exposing an action in which a robot waits until a door opens and then enters inside. + + The server expects the following parameters to be made available on the ROS parameter server: + * movement_duration: Time (in seconds) for which the robot should move + * speed: Movement speed + * move_forward_server: Name of a server exposing an action for moving a base forward -class EnterDoorSkill(smach.StateMachine): - def __init__(self, timeout=10): - smach.StateMachine.__init__(self, - outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED', 'PREEMPTED'], - input_keys=['enter_door_goal'], - output_keys=['enter_door_feedback', 'enter_door_result']) + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de + ''' + def __init__(self): movement_duration = float(rospy.get_param('~movement_duration', 15.)) speed = float(rospy.get_param('~speed', 0.15)) move_forward_server = rospy.get_param('~move_forward_server', 'move_forward_server') - with self: - smach.StateMachine.add('SETUP_ENTER_DOOR', SetupEnterDoor(), - transitions={'succeeded': 'WAIT_FOR_DOOR', - 'failed': 'SETUP_ENTER_DOOR'}) - - smach.StateMachine.add('WAIT_FOR_DOOR', WaitForDoor(), - transitions={'succeeded': 'ENTER_DOOR', - 'waiting': 'WAIT_FOR_DOOR'}) - - smach.StateMachine.add('ENTER_DOOR', EnterDoor(move_forward_server=move_forward_server, - movement_duration=movement_duration, - speed=speed), - transitions={'succeeded': 'SET_ACTION_LIB_SUCCESS', - 'failed': 'SET_ACTION_LIB_FAILED'}) - - smach.StateMachine.add('SET_ACTION_LIB_FAILED', SetActionLibResult(False), - transitions={'succeeded': 'OVERALL_FAILED'}) - - smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), - transitions={'succeeded': 'OVERALL_SUCCESS'}) + rospy.loginfo('[enter_door] Initialising state machine') + self.action_sm = EnterDoorSM(move_forward_server=move_forward_server, + movement_duration=movement_duration, + speed=speed) + rospy.loginfo('[enter_door] State machine initialised') + + self.action_server = actionlib.SimpleActionServer('enter_door_server', + EnterDoorAction, + self.execute, False) + self.action_server.start() + rospy.loginfo('enter_door action server ready') + + def execute(self, goal): + rospy.loginfo('[enter_door] Received an action request') + self.action_sm.goal = goal + self.action_sm.result = None + self.action_sm.execution_requested = True + while not self.action_sm.result: + rospy.sleep(0.05) + self.action_server.set_succeeded(self.action_sm.result) if __name__ == '__main__': rospy.init_node('enter_door_server') - - # construct state machine - sm = EnterDoorSkill() - - # smach viewer - sis = IntrospectionServer('enter_door_smach_viewer', sm, '/enter_door_SMACH_VIEWER') - sis.start() - - asw = ActionServerWrapper( - server_name='enter_door_server', - action_spec=EnterDoorAction, - wrapped_container=sm, - succeeded_outcomes=['OVERALL_SUCCESS'], - aborted_outcomes=['OVERALL_FAILED'], - preempted_outcomes=['PREEMPTED'], - goal_key='enter_door_goal', - feedback_key='enter_door_feedback', - result_key='enter_door_result') - - # Run the server in a background thread - asw.run_server() - rospy.spin() + enter_door_server = EnterDoorServer() + try: + enter_door_server.action_sm.run() + while enter_door_server.action_sm.is_running and not rospy.is_shutdown(): + rospy.spin() + except (KeyboardInterrupt, SystemExit): + print('{0} interrupted; exiting...'.format(enter_door_server.action_sm.name)) + enter_door_server.action_sm.stop() diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/src/mdr_enter_door_action/action_states.py b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/src/mdr_enter_door_action/action_states.py index f76a46336..7325ec5a0 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/src/mdr_enter_door_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_enter_door_action/ros/src/mdr_enter_door_action/action_states.py @@ -1,66 +1,48 @@ #!/usr/bin/python - import rospy -import smach -from actionlib import SimpleActionClient - +import actionlib from std_msgs.msg import Bool -from mdr_enter_door_action.msg import EnterDoorFeedback, EnterDoorResult -from mdr_move_forward_action.msg import MoveForwardAction, MoveForwardGoal - -class SetupEnterDoor(smach.State): - def __init__(self): - smach.State.__init__(self, outcomes=['succeeded', 'failed'], - output_keys=['enter_door_feedback']) - def execute(self, userdata): - feedback = EnterDoorFeedback() - feedback.current_state = 'setup_enter_door' - feedback.message = '[enter_door] entering door' - userdata.enter_door_feedback = feedback +from pyftsm.ftsm import FTSMTransitions +from mas_execution.action_sm_base import ActionSMBase +from mdr_move_forward_action.msg import MoveForwardAction, MoveForwardGoal +from mdr_enter_door_action.msg import EnterDoorFeedback, EnterDoorResult - return 'succeeded' +class EnterDoorSM(ActionSMBase): + def __init__(self, waiting_sleep_duration=1., + door_status_topic='/mdr_perception/door_status/door_status', + timeout=120.0, + move_forward_server='move_forward_server', + movement_duration=10., speed=0.1, + max_recovery_attempts=1): + super(EnterDoorSM, self).__init__('EnterDoor', [], max_recovery_attempts) -class WaitForDoor(smach.State): - def __init__(self, sleep_duration=1., - door_status_topic='/mcr_perception/door_status/door_status'): - smach.State.__init__(self, outcomes=['succeeded', 'waiting'], - output_keys=['enter_door_feedback']) - self.sleep_duration = sleep_duration - self.door_open = False + self.waiting_sleep_duration = waiting_sleep_duration self.door_status_sub = rospy.Subscriber(door_status_topic, Bool, self.update_door_status) - - def execute(self, userdata): - feedback = EnterDoorFeedback() - feedback.current_state = 'wait_for_door' - feedback.message = '[enter_door] waiting for door to open' - userdata.enter_door_feedback = feedback - - rospy.sleep(self.sleep_duration) - - if self.door_open: - return 'succeeded' - return 'waiting' - - def update_door_status(self, msg): - self.door_open = msg.data - -class EnterDoor(smach.State): - def __init__(self, timeout=120.0, - move_forward_server='move_forward_server', - movement_duration=10., speed=0.1): - smach.State.__init__(self, outcomes=['succeeded', 'failed']) self.timeout = timeout self.movement_duration = movement_duration self.speed = speed - self.entered = False + self.move_forward_server = move_forward_server - self.move_forward_client = SimpleActionClient(move_forward_server, - MoveForwardAction) - self.move_forward_client.wait_for_server() + self.door_open = False + self.move_forward_client = None + + def init(self): + try: + self.move_forward_client = actionlib.SimpleActionClient(self.move_forward_server, MoveForwardAction) + rospy.loginfo('[enter_door] Waiting for %s server', self.move_forward_server) + self.move_forward_client.wait_for_server() + except: + rospy.logerr('[enter_door] %s server does not seem to respond', self.move_forward_server) + return FTSMTransitions.INITIALISED + + def running(self): + rospy.loginfo('[enter_door] Waiting for door to open') + while not self.door_open: + rospy.sleep(self.waiting_sleep_duration) + rospy.loginfo('[enter_door] Door open; entering') - def execute(self, userdata): goal = MoveForwardGoal() goal.movement_duration = self.movement_duration goal.speed = self.speed @@ -68,20 +50,16 @@ def execute(self, userdata): self.move_forward_client.send_goal(goal) timeout_duration = rospy.Duration.from_sec(self.timeout) self.move_forward_client.wait_for_result(timeout_duration) - result = self.move_forward_client.get_result() if result and result.success: - return 'succeeded' - return 'failed' + self.result = self.set_result(True) + self.result = self.set_result(False) + return FTSMTransitions.DONE -class SetActionLibResult(smach.State): - def __init__(self, result): - smach.State.__init__(self, outcomes=['succeeded'], - output_keys=['enter_door_result']) - self.result = result + def update_door_status(self, msg): + self.door_open = msg.data - def execute(self, userdata): + def set_result(self, success): result = EnterDoorResult() - result.success = self.result - userdata.enter_door_result = result - return 'succeeded' + result.success = success + return result diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_follow_person/CMakeLists.txt b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_follow_person/CMakeLists.txt index 7a247c43d..701cfc162 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_follow_person/CMakeLists.txt +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_follow_person/CMakeLists.txt @@ -7,7 +7,7 @@ find_package(catkin REQUIRED geometry_msgs nav_msgs mcr_algorithms - mcr_perception_msgs + mas_perception_msgs tf std_srvs ) @@ -17,7 +17,7 @@ catkin_package( geometry_msgs nav_msgs mcr_algorithms - mcr_perception_msgs + mas_perception_msgs std_srvs ) diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_follow_person/package.xml b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_follow_person/package.xml index 1b07371be..1f5a2b44d 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_follow_person/package.xml +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_follow_person/package.xml @@ -16,14 +16,14 @@ geometry_msgs nav_msgs mcr_algorithms - mcr_perception_msgs + mas_perception_msgs tf std_srvs geometry_msgs nav_msgs mcr_algorithms - mcr_perception_msgs + mas_perception_msgs std_srvs roslaunch diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_follow_person/ros/src/follow_person_node.cpp b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_follow_person/ros/src/follow_person_node.cpp index f2e6a268a..fa9706384 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_follow_person/ros/src/follow_person_node.cpp +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_follow_person/ros/src/follow_person_node.cpp @@ -15,8 +15,8 @@ #include #include -#include -#include +#include +#include enum EnumBehaviorState { @@ -31,7 +31,7 @@ class PersonFollowBehavior PersonFollowBehavior(ros::NodeHandle &nh); ~PersonFollowBehavior(); - void personTrackerCallback(const mcr_perception_msgs::PersonList::ConstPtr &inputPersonList); + void personTrackerCallback(const mas_perception_msgs::PersonList::ConstPtr &inputPersonList); void odometryCallback(const nav_msgs::Odometry::ConstPtr &odom_msgs); void nextStep(); @@ -51,7 +51,7 @@ class PersonFollowBehavior PIController* pi_controller_x_; PIController* pi_controller_theta_; tf::TransformListener* _pTransformListener; - mcr_perception_msgs::PersonList _personList; + mas_perception_msgs::PersonList _personList; nav_msgs::Odometry current_odom_; double _dMaxTranslationalVelocity; double _dMaxRotationalVelocity; @@ -88,7 +88,7 @@ PersonFollowBehavior::PersonFollowBehavior(ros::NodeHandle &nh) this->_eState = eBehaviorStateInit; // subscribe and advertise - this->_subPersonTracker = this->_nh->subscribe < mcr_perception_msgs::PersonList + this->_subPersonTracker = this->_nh->subscribe < mas_perception_msgs::PersonList > ("people_positions", 1, &PersonFollowBehavior::personTrackerCallback, this); this->_subOdom = this->_nh->subscribe < nav_msgs::Odometry > ("odometry", 1, &PersonFollowBehavior::odometryCallback, this); this->_pubBaseCommands = this->_nh->advertise < geometry_msgs::Twist > ("cmd_vel", 1); @@ -370,7 +370,7 @@ void PersonFollowBehavior::nextStep() _dLastSpeed = fabs(this->_dTranslationalVelocityToSet) + fabs(this->_dRotationalVelocityToSet); } -void PersonFollowBehavior::personTrackerCallback(const mcr_perception_msgs::PersonList::ConstPtr& inputPersonList) +void PersonFollowBehavior::personTrackerCallback(const mas_perception_msgs::PersonList::ConstPtr& inputPersonList) { this->_personList = *inputPersonList; } diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/README.md b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/README.md index 32d626da2..7540ca566 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/README.md +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/README.md @@ -76,8 +76,6 @@ The following parameters need to be passed when launching the action client: * ``action_timeout``: Maximum time (in seconds) that we are willing to wait for the action to be executed * ``action_dispatch_topic``: Name of the topic at which the plan dispatcher sends action requests * ``action_feedback_topic``: Name of the topic at which the action sends feedback to the plan dispatcher -* ``knowledge_update_service``: Name of a service used for updating the planning problem as the world changes -* ``attribute_fetching_service``: Name of a service used for retrieving attributes representing the current knowledge about the world ## Action execution summary diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/launch/move_base_action.launch b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/launch/move_base_action.launch index b06d73a8f..d0630f1da 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/launch/move_base_action.launch +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/launch/move_base_action.launch @@ -3,7 +3,7 @@ - + diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/launch/move_base_client.launch b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/launch/move_base_client.launch index 3cd0c3e57..10a356294 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/launch/move_base_client.launch +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/launch/move_base_client.launch @@ -2,8 +2,6 @@ - - @@ -11,7 +9,5 @@ - - diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_action b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_action index f31be9ea1..2379ab34f 100755 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_action +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_action @@ -1,57 +1,64 @@ #!/usr/bin/env python import rospy -import smach - -import smach_ros -from mdr_move_base_action.msg import MoveBaseAction, MoveBaseResult -from mdr_move_base_action.action_states import SetupMoveBase, ApproachPose, SetActionLibResult - -def main(): - rospy.init_node('mdr_move_base_action_server') - - move_base_server = rospy.get_param('~move_base_server', '') - pose_description_file = rospy.get_param('~pose_description_file', '') - pose_frame = rospy.get_param('~pose_frame', '') - safe_arm_joint_config = rospy.get_param('~safe_arm_joint_config', 'folded') - move_arm_server = rospy.get_param('~move_arm_server', 'move_arm_server') - - sm = smach.StateMachine(outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'], - input_keys=['move_base_goal'], - output_keys=['move_base_feedback', 'move_base_result']) - - with sm: - smach.StateMachine.add('SETUP_MOVE_BASE', - SetupMoveBase(safe_arm_joint_config=safe_arm_joint_config, - move_arm_server=move_arm_server), - transitions={'succeeded': 'MOVE_BASE_TO_DESTINATION', - 'failed': 'SETUP_MOVE_BASE'}) - - smach.StateMachine.add('MOVE_BASE_TO_DESTINATION', - ApproachPose(move_base_server=move_base_server, - pose_description_file=pose_description_file, - pose_frame=pose_frame), - transitions={'succeeded': 'SET_ACTION_LIB_SUCCESS', - 'failed': 'SET_ACTION_LIB_FAILED'}) - - smach.StateMachine.add('SET_ACTION_LIB_FAILED', SetActionLibResult(False), - transitions={'succeeded': 'OVERALL_FAILED'}) - - smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), - transitions={'succeeded': 'OVERALL_SUCCESS'}) - - asw = smach_ros.ActionServerWrapper( - server_name='move_base_server', - action_spec=MoveBaseAction, - wrapped_container=sm, - succeeded_outcomes=['OVERALL_SUCCESS'], - aborted_outcomes=['OVERALL_FAILED'], - preempted_outcomes=['PREEMPTED'], - goal_key='move_base_goal', - feedback_key='move_base_feedback', - result_key='move_base_result') - - asw.run_server() - rospy.spin() +import actionlib + +from mdr_move_base_action.msg import MoveBaseAction +from mdr_move_base_action.action_states import MoveBaseSM + +class MoveBaseServer(object): + '''A server exposing a go to action. + + The server expects the following parameters to be made available on the ROS parameter server: + * move_base_server: Name of the default move_base server (default: '/move_base') + * pose_description_file: Name of a yaml file in which named goals are mapped + to actual coordinates + * pose_frame: Name of the frame in which the poses in pose_description_file + are given (default: 'map') + * safe_arm_joint_config: Name of a configuration specified in which the robot can + safely move around the environment (default: 'folded') + * move_arm_server: Name of the move_arm action server (default: 'move_arm_server') + + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de + + ''' + def __init__(self): + safe_arm_joint_config = rospy.get_param('~safe_arm_joint_config', 'folded') + move_arm_server = rospy.get_param('~move_arm_server', 'move_arm_server') + move_base_server = rospy.get_param('~move_base_server', '') + pose_description_file = rospy.get_param('~pose_description_file', '') + pose_frame = rospy.get_param('~pose_frame', '') + + rospy.loginfo('[move_base] Initialising state machine') + self.action_sm = MoveBaseSM(safe_arm_joint_config=safe_arm_joint_config, + move_arm_server=move_arm_server, + move_base_server=move_base_server, + pose_description_file=pose_description_file, + pose_frame=pose_frame) + rospy.loginfo('[move_base] State machine initialised') + + self.action_server = actionlib.SimpleActionServer('move_base_server', + MoveBaseAction, + self.execute, False) + self.action_server.start() + rospy.loginfo('move_base action server ready') + + def execute(self, goal): + rospy.loginfo('[move_base] Received an action request') + self.action_sm.goal = goal + self.action_sm.result = None + self.action_sm.execution_requested = True + while not self.action_sm.result: + rospy.sleep(0.05) + self.action_server.set_succeeded(self.action_sm.result) if __name__ == '__main__': - main() + rospy.init_node('move_base_server') + move_base_server = MoveBaseServer() + try: + move_base_server.action_sm.run() + while move_base_server.action_sm.is_running and not rospy.is_shutdown(): + rospy.spin() + except (KeyboardInterrupt, SystemExit): + print('{0} interrupted; exiting...'.format(move_base_server.action_sm.name)) + move_base_server.action_sm.stop() diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_action_client_test b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_action_client_test index e7f8a06da..f08640e86 100755 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_action_client_test +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_action_client_test @@ -8,7 +8,7 @@ from mdr_move_base_action.msg import MoveBaseAction, MoveBaseGoal if __name__ == '__main__': rospy.init_node('mdr_move_base_client_test') - client = actionlib.SimpleActionClient('/mdr_actions/move_base_server', MoveBaseAction) + client = actionlib.SimpleActionClient('move_base_server', MoveBaseAction) client.wait_for_server() goal = MoveBaseGoal() if len(sys.argv) == 2: diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_client b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_client index 47d891e3f..f07df28e5 100755 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_client +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/scripts/move_base_client @@ -2,9 +2,6 @@ import rospy import actionlib -import rosplan_knowledge_msgs.srv as rosplan_srvs -import diagnostic_msgs.msg as diag_msgs - from mdr_rosplan_interface.action_client_base import ActionClientBase from mdr_move_base_action.msg import MoveBaseAction, MoveBaseGoal @@ -56,41 +53,13 @@ class MoveBaseClient(ActionClientBase): return goal def update_knowledge_base(self, destination_location): - # we add the fact that the robot is not at the source location to the knowledge base - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 2 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'robot_at' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - request.knowledge.values.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'wp' - arg_msg.value = self.original_location - request.knowledge.values.append(arg_msg) - - self.knowledge_update_client(request) - - # we add the fact that the robot is at the destination location to the knowledge base - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 0 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'robot_at' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - request.knowledge.values.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'wp' - arg_msg.value = destination_location - request.knowledge.values.append(arg_msg) - - self.knowledge_update_client(request) + '''Updates the knowledge base with the following facts: + * the robot is not at the original location anymore + * the robot is at the destination location + ''' + facts_to_add = [('robot_at', [('bot', self.robot_name), ('wp', destination_location)])] + facts_to_remove = [('robot_at', [('bot', self.robot_name), ('wp', self.original_location)])] + self.kb_interface.update_kb(facts_to_add, facts_to_remove) if __name__ == '__main__': rospy.init_node('mdr_move_base_client') diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/src/mdr_move_base_action/action_states.py b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/src/mdr_move_base_action/action_states.py index fe7851287..f504e3f46 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/src/mdr_move_base_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_base_action/ros/src/mdr_move_base_action/action_states.py @@ -1,63 +1,54 @@ -#!/usr/bin/python - +import yaml import rospy -import smach import actionlib -import yaml import tf from geometry_msgs.msg import PoseStamped, Quaternion import move_base_msgs.msg as move_base_msgs +from pyftsm.ftsm import FTSMTransitions +from mas_execution.action_sm_base import ActionSMBase from mdr_move_arm_action.msg import MoveArmAction, MoveArmGoal from mdr_move_base_action.msg import MoveBaseGoal, MoveBaseFeedback, MoveBaseResult -class SetupMoveBase(smach.State): - def __init__(self, safe_arm_joint_config='folded', move_arm_server='move_arm_server'): - smach.State.__init__(self, outcomes=['succeeded', 'failed'], - input_keys=['move_base_goal'], - output_keys=['move_base_feedback', 'move_base_result']) +class MoveBaseSM(ActionSMBase): + def __init__(self, timeout=120., + safe_arm_joint_config='folded', + move_arm_server='move_arm_server', + move_base_server='/move_base', + pose_description_file='', + pose_frame='map', + max_recovery_attempts=1): + super(MoveBaseSM, self).__init__('MoveBase', [], max_recovery_attempts) + self.pose = None self.safe_arm_joint_config = safe_arm_joint_config self.move_arm_server = move_arm_server - self.move_arm_client = actionlib.SimpleActionClient(self.move_arm_server, MoveArmAction) - self.move_arm_client.wait_for_server() + self.move_base_server = move_base_server + self.pose_description_file = pose_description_file + self.pose_frame = pose_frame + self.timeout = timeout + self.move_arm_client = None - def execute(self, userdata): - feedback = MoveBaseFeedback() - feedback.current_state = 'SETUP_MOVE_BASE' - feedback.message = '[MOVE_BASE] Received a move base request' - userdata.move_base_feedback = feedback + def init(self): + try: + self.move_arm_client = actionlib.SimpleActionClient(self.move_arm_server, MoveArmAction) + rospy.loginfo('[move_base] Waiting for %s server', self.move_arm_server) + self.move_arm_client.wait_for_server() + except: + rospy.logerr('[move_base] %s server does not seem to respond', self.move_arm_server) + return FTSMTransitions.INITIALISED - rospy.loginfo('[MOVE_BASE] Moving the arm to a safe configuration...') + def running(self): + rospy.loginfo('[move_base] Moving the arm to a safe configuration...') move_arm_goal = MoveArmGoal() move_arm_goal.goal_type = MoveArmGoal.NAMED_TARGET move_arm_goal.named_target = self.safe_arm_joint_config self.move_arm_client.send_goal(move_arm_goal) self.move_arm_client.wait_for_result() - return 'succeeded' -class ApproachPose(smach.State): - def __init__(self, timeout=120., - move_base_server='/move_base', - pose_description_file='', - pose_frame='map'): - smach.State.__init__(self, outcomes=['succeeded', 'failed'], - input_keys=['move_base_goal'], - output_keys=['move_base_feedback', 'move_base_result']) - self.pose = None - self.move_base_server = move_base_server - self.pose_description_file = pose_description_file - self.pose_frame = pose_frame - self.timeout = timeout - - def execute(self, userdata): pose = PoseStamped() - if userdata.move_base_goal.goal_type == MoveBaseGoal.NAMED_TARGET: - destination = userdata.move_base_goal.destination_location - - feedback = MoveBaseFeedback() - feedback.current_state = 'APPROACH_POSE' - feedback.message = '[MOVE_BASE] Moving base to ' + destination - userdata.move_base_feedback = feedback + if self.goal.goal_type == MoveBaseGoal.NAMED_TARGET: + destination = self.goal.destination_location + rospy.loginfo('[move_base] Moving base to %s', destination) self.pose = self.convert_pose_name_to_coordinates(destination) pose.header.stamp = rospy.Time.now() @@ -67,16 +58,13 @@ def execute(self, userdata): quat = tf.transformations.quaternion_from_euler(0, 0, self.pose[2]) pose.pose.orientation = Quaternion(*quat) - elif userdata.move_base_goal.goal_type == MoveBaseGoal.POSE: - pose = userdata.move_base_goal.pose - - feedback = MoveBaseFeedback() - feedback.current_state = 'APPROACH_POSE' - feedback.message = '[MOVE_BASE] Moving base to {0}'.format(pose) - userdata.move_base_feedback = feedback + elif self.goal.goal_type == MoveBaseGoal.POSE: + pose = self.goal.pose + rospy.loginfo('[move_base] Moving base to %s', pose) else: - rospy.logerr('[MOVE_BASE] Received an unknown goal type; ignoring request') - return 'failed' + rospy.logerr('[move_base] Received an unknown goal type; ignoring request') + self.result = self.set_result(False) + return FTSMTransitions.DONE goal = move_base_msgs.MoveBaseGoal() goal.target_pose = pose @@ -88,13 +76,16 @@ def execute(self, userdata): success = move_base_client.wait_for_result() if success: - rospy.loginfo('Pose reached successfully') - return 'succeeded' - rospy.logerr('Pose could not be reached') - return 'failed' + rospy.loginfo('[move_base] Pose reached successfully') + self.result = self.set_result(True) + return FTSMTransitions.DONE + + rospy.logerr('[move_base] Pose could not be reached') + self.result = self.set_result(False) + return FTSMTransitions.DONE def convert_pose_name_to_coordinates(self, pose_name): - stream = file(self.pose_description_file, 'r') + stream = open(self.pose_description_file, 'r') poses = yaml.load(stream) stream.close() try: @@ -104,15 +95,7 @@ def convert_pose_name_to_coordinates(self, pose_name): rospy.logerr('Pose name "%s" does not exist' % (pose_name)) return None -class SetActionLibResult(smach.State): - def __init__(self, result): - smach.State.__init__(self, outcomes=['succeeded'], - input_keys=['move_base_goal'], - output_keys=['move_base_feedback', 'move_base_result']) - self.result = result - - def execute(self, userdata): + def set_result(self, success): result = MoveBaseResult() - result.success = self.result - userdata.move_base_result = result - return 'succeeded' + result.success = success + return result diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_forward_action/ros/scripts/move_forward_action b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_forward_action/ros/scripts/move_forward_action index 135f87d7e..4a7208c7e 100755 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_forward_action/ros/scripts/move_forward_action +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_forward_action/ros/scripts/move_forward_action @@ -1,58 +1,48 @@ #!/usr/bin/env python import rospy -import smach - -from smach_ros import ActionServerWrapper, IntrospectionServer +import actionlib from mdr_move_forward_action.msg import MoveForwardAction -from mdr_move_forward_action.action_states import SetupMoveForward, MoveForward, SetActionLibResult - -class MoveForwardSkill(smach.StateMachine): - def __init__(self, timeout=10): - smach.StateMachine.__init__(self, - outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED', 'PREEMPTED'], - input_keys=['move_forward_goal'], - output_keys=['move_forward_feedback', 'move_forward_result']) - - velocity_topic = rospy.get_param('~velocity_topic', '/cmd_vel') +from mdr_move_forward_action.action_states import MoveForwardSM - with self: - smach.StateMachine.add('SETUP_MOVE_FORWARD', SetupMoveForward(), - transitions={'succeeded': 'MOVE_FORWARD', - 'failed': 'SETUP_MOVE_FORWARD'}) +class MoveForwardServer(object): + '''A server exposing an action for moving a base forward and backward. - smach.StateMachine.add('MOVE_FORWARD', MoveForward(velocity_topic=velocity_topic), - transitions={'succeeded': 'SET_ACTION_LIB_SUCCESS', - 'failed': 'SET_ACTION_LIB_FAILED'}) + The server expects the following parameters to be made available on the ROS parameter server: + * velocity_topic: Name of a topic on which velocity commands are published - smach.StateMachine.add('SET_ACTION_LIB_FAILED', SetActionLibResult(False), - transitions={'succeeded': 'OVERALL_FAILED'}) + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de - smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), - transitions={'succeeded': 'OVERALL_SUCCESS'}) + ''' + def __init__(self): + velocity_topic = rospy.get_param('~velocity_topic', '/cmd_vel') + rospy.loginfo('[move_forward] Initialising state machine') + self.action_sm = MoveForwardSM(velocity_topic=velocity_topic) + rospy.loginfo('[move_forward] State machine initialised') + + self.action_server = actionlib.SimpleActionServer('move_forward_server', + MoveForwardAction, + self.execute, False) + self.action_server.start() + rospy.loginfo('move_forward action server ready') + + def execute(self, goal): + rospy.loginfo('[move_forward] Received an action request') + self.action_sm.goal = goal + self.action_sm.result = None + self.action_sm.execution_requested = True + while not self.action_sm.result: + rospy.sleep(0.05) + self.action_server.set_succeeded(self.action_sm.result) if __name__ == '__main__': rospy.init_node('move_forward_server') - - # construct state machine - sm = MoveForwardSkill() - - # smach viewer - sis = IntrospectionServer('move_forward_smach_viewer', sm, - '/MOVE_FORWARD_SMACH_VIEWER') - sis.start() - - asw = ActionServerWrapper( - server_name='move_forward_server', - action_spec=MoveForwardAction, - wrapped_container=sm, - succeeded_outcomes=['OVERALL_SUCCESS'], - aborted_outcomes=['OVERALL_FAILED'], - preempted_outcomes=['PREEMPTED'], - goal_key='move_forward_goal', - feedback_key='move_forward_feedback', - result_key='move_forward_result') - - # Run the server in a background thread - asw.run_server() - rospy.spin() + move_forward_server = MoveForwardServer() + try: + move_forward_server.action_sm.run() + while move_forward_server.action_sm.is_running and not rospy.is_shutdown(): + rospy.spin() + except (KeyboardInterrupt, SystemExit): + print('{0} interrupted; exiting...'.format(move_forward_server.action_sm.name)) + move_forward_server.action_sm.stop() diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_forward_action/ros/src/mdr_move_forward_action/action_states.py b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_forward_action/ros/src/mdr_move_forward_action/action_states.py index be7fc7a89..752258f93 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_forward_action/ros/src/mdr_move_forward_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_move_forward_action/ros/src/mdr_move_forward_action/action_states.py @@ -1,48 +1,26 @@ #!/usr/bin/python - import rospy -import smach -import smach_ros -import actionlib +from pyftsm.ftsm import FTSMTransitions +from mas_execution.action_sm_base import ActionSMBase from geometry_msgs.msg import Twist from mdr_move_forward_action.msg import MoveForwardFeedback, MoveForwardResult - -class SetupMoveForward(smach.State): - def __init__(self): - smach.State.__init__(self, outcomes=['succeeded', 'failed'], - input_keys=['move_forward_goal'], - output_keys=['move_forward_feedback', 'move_forward_result']) - - def execute(self, userdata): - duration = userdata.move_forward_goal.movement_duration - speed = userdata.move_forward_goal.speed - - feedback = MoveForwardFeedback() - feedback.current_state = 'MOVE_FORWARD' - feedback.message = '[move_forward] moving the base forward for ' + str(duration) + 's at ' + str(speed) + 'm/s' - userdata.move_forward_feedback = feedback - - return 'succeeded' - - -class MoveForward(smach.State): - def __init__(self, timeout=120.0, - velocity_topic='/cmd_vel'): - smach.State.__init__(self, input_keys=['move_forward_goal'], - outcomes=['succeeded', 'failed']) +class MoveForwardSM(ActionSMBase): + def __init__(self, timeout=120.0, velocity_topic='/cmd_vel', max_recovery_attempts=1): + super(MoveForwardSM, self).__init__('MoveForward', [], max_recovery_attempts) self.timeout = timeout self.velocity_pub = rospy.Publisher(velocity_topic, Twist) - def execute(self, userdata): - duration = rospy.Duration.from_sec(userdata.move_forward_goal.movement_duration) - speed = userdata.move_forward_goal.speed + def running(self): + duration = rospy.Duration.from_sec(self.goal.movement_duration) + speed = self.goal.speed rate = rospy.Rate(5) twist = Twist() twist.linear.x = speed + rospy.loginfo('[move_forward] moving the base forward for %s s at %s m/s', str(duration), str(speed)) start_time = rospy.Time.now() while (rospy.Time.now() - start_time) < duration: self.velocity_pub.publish(twist) @@ -51,19 +29,10 @@ def execute(self, userdata): # we publish a zero twist at the end so that the robot stops moving zero_twist = Twist() self.velocity_pub.publish(zero_twist) + self.result = self.set_result(True) + return FTSMTransitions.DONE - return 'succeeded' - - -class SetActionLibResult(smach.State): - def __init__(self, result): - smach.State.__init__(self, outcomes=['succeeded'], - input_keys=['move_forward_goal'], - output_keys=['move_forward_feedback', 'move_forward_result']) - self.result = result - - def execute(self, userdata): + def set_result(self, success): result = MoveForwardResult() - result.success = self.result - userdata.move_forward_result = result - return 'succeeded' + result.success = success + return result diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_turn_base_to_action/ros/scripts/turn_base_to_action b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_turn_base_to_action/ros/scripts/turn_base_to_action index e08875e54..e22fe40e6 100755 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_turn_base_to_action/ros/scripts/turn_base_to_action +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_turn_base_to_action/ros/scripts/turn_base_to_action @@ -1,59 +1,52 @@ #!/usr/bin/env python import rospy -import smach - -from smach_ros import ActionServerWrapper, IntrospectionServer +import actionlib from mdr_turn_base_to_action.msg import TurnBaseToAction -from mdr_turn_base_to_action.action_states import SetupTurnBaseTo, TurnBaseTo, SetActionLibResult - -class TurnBaseToSkill(smach.StateMachine): - def __init__(self, timeout=10): - smach.StateMachine.__init__(self, - outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED', 'PREEMPTED'], - input_keys=['turn_base_to_goal'], - output_keys=['turn_base_to_feedback', 'turn_base_to_result']) +from mdr_turn_base_to_action.action_states import TurnBaseSM - move_base_server_name = rospy.get_param('~move_base_server', '/move_base') - rotation_frame = rospy.get_param('~rotation_frame', 'base_link') +class TurnBaseServer(object): + '''A server exposing an action for turning a robot base to a desired angle. - with self: - smach.StateMachine.add('SETUP_TURN_BASE_TO', SetupTurnBaseTo(), - transitions={'succeeded':'TURN_BASE_TO', - 'failed':'SET_ACTION_LIB_FAILED'}) + The server expects the following parameters to be made available on the ROS parameter server: + * move_base_server: Name of an action server for moving the base + * rotation_frame: Name of the frame in which the rotation is performed - smach.StateMachine.add('TURN_BASE_TO', TurnBaseTo(move_base_server=move_base_server_name, - rotation_frame=rotation_frame), - transitions={'succeeded': 'SET_ACTION_LIB_SUCCESS', - 'failed': 'SET_ACTION_LIB_FAILED'}) + @author Alex Mitrevski, Jose Mayoral Banos + @contact aleksandar.mitrevski@h-brs.de - smach.StateMachine.add('SET_ACTION_LIB_FAILED', SetActionLibResult(False), - transitions={'succeeded':'OVERALL_FAILED'}) + ''' + def __init__(self): + move_base_server_name = rospy.get_param('~move_base_server', '/move_base') + rotation_frame = rospy.get_param('~rotation_frame', 'base_link') - smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), - transitions={'succeeded':'OVERALL_SUCCESS'}) + rospy.loginfo('[turn_base] Initialising state machine') + self.action_sm = TurnBaseSM(move_base_server=move_base_server_name, + rotation_frame=rotation_frame) + rospy.loginfo('[turn_base] State machine initialised') + + self.action_server = actionlib.SimpleActionServer('turn_base_to_server', + TurnBaseToAction, + self.execute, False) + self.action_server.start() + rospy.loginfo('turn_base action server ready') + + def execute(self, goal): + rospy.loginfo('[turn_base] Received an action request') + self.action_sm.goal = goal + self.action_sm.result = None + self.action_sm.execution_requested = True + while not self.action_sm.result: + rospy.sleep(0.05) + self.action_server.set_succeeded(self.action_sm.result) if __name__ == '__main__': - rospy.init_node('turn_base_to_server') - - # construct state machine - sm = TurnBaseToSkill() - - # smach viewer - sis = IntrospectionServer('turn_base_to_smach_viewer', sm, '/TURN_BASE_TO_SMACH_VIEWER') - sis.start() - - asw = ActionServerWrapper( - server_name='turn_base_to_server', - action_spec=TurnBaseToAction, - wrapped_container=sm, - succeeded_outcomes=['OVERALL_SUCCESS'], - aborted_outcomes=['OVERALL_FAILED'], - preempted_outcomes=['PREEMPTED'], - goal_key='turn_base_to_goal', - feedback_key='turn_base_to_feedback', - result_key='turn_base_to_result') - - # Run the server in a background thread - asw.run_server() - rospy.spin() + rospy.init_node('turn_base_server') + turn_base_server = TurnBaseServer() + try: + turn_base_server.action_sm.run() + while turn_base_server.action_sm.is_running and not rospy.is_shutdown(): + rospy.spin() + except (KeyboardInterrupt, SystemExit): + print('{0} interrupted; exiting...'.format(turn_base_server.action_sm.name)) + turn_base_server.action_sm.stop() diff --git a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_turn_base_to_action/ros/src/mdr_turn_base_to_action/action_states.py b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_turn_base_to_action/ros/src/mdr_turn_base_to_action/action_states.py index 3c0c1bb9c..b094c38ed 100644 --- a/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_turn_base_to_action/ros/src/mdr_turn_base_to_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_navigation_actions/mdr_turn_base_to_action/ros/src/mdr_turn_base_to_action/action_states.py @@ -1,70 +1,53 @@ #!/usr/bin/python - import rospy -import smach import actionlib from tf.transformations import quaternion_from_euler -from mdr_turn_base_to_action.msg import TurnBaseToFeedback, TurnBaseToResult +from pyftsm.ftsm import FTSMTransitions +from mas_execution.action_sm_base import ActionSMBase from mdr_move_base_action.msg import MoveBaseAction, MoveBaseGoal +from mdr_turn_base_to_action.msg import TurnBaseToFeedback, TurnBaseToResult - -class SetupTurnBaseTo(smach.State): - def __init__(self): - smach.State.__init__(self, outcomes=['succeeded', 'failed'], - output_keys=['turn_base_to_feedback']) - - def execute(self, userdata): - feedback = TurnBaseToFeedback() - feedback.message = '[turn_base_to_goal] sm initialize' - userdata.turn_base_to_feedback = feedback - return 'succeeded' - - -class TurnBaseTo(smach.State): +class TurnBaseSM(ActionSMBase): def __init__(self, timeout=120.0, rotation_frame='base_link', move_base_server='/move_base', - movement_duration=15., speed=0.1): - smach.State.__init__(self, input_keys=['turn_base_to_goal'], - outcomes=['succeeded', 'failed']) - - rospy.loginfo("Using move base server: " + move_base_server) - rospy.loginfo("Rotation Frame set to : " + rotation_frame) + max_recovery_attempts=1): + super(TurnBaseSM, self).__init__('TurnBase', [], max_recovery_attempts) self.move_base_server = move_base_server self.timeout = timeout self.rotation_frame = rotation_frame - self.movement_duration = movement_duration - self.speed = speed - - self.move_base_client = actionlib.SimpleActionClient(self.move_base_server, MoveBaseAction) - self.move_base_client.wait_for_server() - - def execute(self, userdata): + self.move_base_client = None + + def init(self): + try: + self.move_base_client = actionlib.SimpleActionClient(self.move_base_server, MoveBaseAction) + rospy.loginfo('[turn_base_to] Waiting for %s server', self.move_base_server) + self.move_base_client.wait_for_server() + except: + rospy.logerr('[turn_base_to] %s server does not seem to respond', self.move_base_server) + return FTSMTransitions.INITIALISED + + def running(self): goal = MoveBaseGoal() goal.goal_type = MoveBaseGoal.POSE goal.pose.header.frame_id = self.rotation_frame - q = quaternion_from_euler(0, 0, userdata.turn_base_to_goal.desired_yaw) + q = quaternion_from_euler(0, 0, self.goal.desired_yaw) goal.pose.pose.orientation.x = q[0] goal.pose.pose.orientation.y = q[1] goal.pose.pose.orientation.z = q[2] goal.pose.pose.orientation.w = q[3] - rospy.loginfo("[mdr_turn_base_to] Goal %s", goal) + + rospy.loginfo('[turn_base_to] Rotating the base to %s', str(self.goal.desired_yaw)) self.move_base_client.send_goal(goal) success = self.move_base_client.wait_for_result() if success: - return 'succeeded' - return 'failed' - -class SetActionLibResult(smach.State): - def __init__(self, result): - smach.State.__init__(self, outcomes=['succeeded'], - output_keys=['turn_base_to_result']) - self.result = result + self.result = self.set_result(True) + self.result = self.set_result(False) + return FTSMTransitions.DONE - def execute(self, userdata): + def set_result(self, success): result = TurnBaseToResult() - result.success = self.result - userdata.turn_base_to_result = result - return 'succeeded' + result.success = success + return result diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/scripts/detect_person_action b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/scripts/detect_person_action index a11532abb..51d333734 100755 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/scripts/detect_person_action +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/scripts/detect_person_action @@ -1,33 +1,52 @@ #!/usr/bin/env python import rospy -import smach -from smach_ros import ActionServerWrapper, IntrospectionServer +import actionlib -from mdr_detect_person.skill import DetectPersonSkill from mdr_detect_person.msg import DetectPersonAction +from mdr_detect_person.action_states import DetectPersonSM + +class DetectPersonServer(object): + '''A server exposing an action for detecting person faces in an image. + + The server expects the following parameters to be made available on the ROS parameter server: + * image_topic: Name of a camera image topic (default /cam3d/rgb/image_raw) + * detection_model_path: Absolute path of a person detection model (default '') + + @author Alex Mitrevski, Gabriela Cortes, Octavio Arriaga + @contact aleksandar.mitrevski@h-brs.de + + ''' + def __init__(self): + image_topic = rospy.get_param('~image_topic', '/cam3d/rgb/image_raw') + detection_model_path = rospy.get_param('~config_file', '') + + rospy.loginfo('[detect_person] Initialising state machine') + self.action_sm = DetectPersonSM(image_topic=image_topic, + detection_model_path=detection_model_path) + rospy.loginfo('[detect_person] State machine initialised') + + self.action_server = actionlib.SimpleActionServer('detect_person_server', + DetectPersonAction, + self.execute, False) + self.action_server.start() + rospy.loginfo('detect_person action server ready') + + def execute(self, goal): + rospy.loginfo('[detect_person] Received an action request') + self.action_sm.goal = goal + self.action_sm.result = None + self.action_sm.execution_requested = True + while not self.action_sm.result: + rospy.sleep(0.05) + self.action_server.set_succeeded(self.action_sm.result) if __name__ == '__main__': rospy.init_node('detect_person_server') - - # construct state machine - sm = DetectPersonSkill() - - # smach viewer - sis = IntrospectionServer('detect_person_smach_viewer', sm, - '/DETECT_PERSON_SMACH_VIEWER') - sis.start() - - asw = ActionServerWrapper( - server_name='detect_person_server', - action_spec=DetectPersonAction, - wrapped_container=sm, - succeeded_outcomes=['OVERALL_SUCCESS'], - aborted_outcomes=['OVERALL_FAILED'], - preempted_outcomes=['PREEMPTED'], - goal_key='detect_person_goal', - feedback_key='detect_person_feedback', - result_key='detect_person_result') - - # Run the server in a background thread - asw.run_server() - rospy.spin() + detect_person_server = DetectPersonServer() + try: + detect_person_server.action_sm.run() + while detect_person_server.action_sm.is_running and not rospy.is_shutdown(): + rospy.spin() + except (KeyboardInterrupt, SystemExit): + print('{0} interrupted; exiting...'.format(detect_person_server.action_sm.name)) + detect_person_server.action_sm.stop() diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/src/mdr_detect_person/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/src/mdr_detect_person/action_states.py index 7e2aa355c..c9085e0c2 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/src/mdr_detect_person/action_states.py +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/src/mdr_detect_person/action_states.py @@ -1,97 +1,71 @@ #!/usr/bin/python - import rospy -import smach -import smach_ros -import actionlib import cv2 import numpy as np from sensor_msgs.msg import Image -from cv_bridge import CvBridge, CvBridgeError +from cv_bridge import CvBridge +from pyftsm.ftsm import FTSMTransitions +from mas_execution.action_sm_base import ActionSMBase from mdr_detect_person.msg import DetectPersonFeedback, DetectPersonGoal, DetectPersonResult from mdr_perception_msgs.msg import FaceBoundingBox - -from mdr_detect_person.inference import detect_faces -from mdr_detect_person.inference import load_detection_model - - -class SetupDetectPerson(smach.State): - def __init__(self): - smach.State.__init__(self, outcomes=['succeeded', 'failed'], - input_keys=['detect_person_goal'], - output_keys=['detect_person_feedback', 'detect_person_result']) - - def execute(self, userdata): - start = userdata.detect_person_goal.start - print (start) - if start: - feedback = DetectPersonFeedback() - feedback.current_state = 'DETECT_PERSON' - feedback.text = '[detect_person] detecting faces' - userdata.detect_person_feedback = feedback - return 'succeeded' - else: - return 'failed' - - -class DetectPerson(smach.State): - def __init__(self, timeout=120., image_topic='/cam3d/rgb/image_raw', detection_model_path=''): - smach.State.__init__(self, outcomes=['succeeded', 'failed'], - input_keys=['detect_person_goal', 'number_of_faces', - 'bounding_boxes'], - output_keys=['detect_person_feedback', 'number_of_faces', - 'bounding_boxes']) +from mdr_detect_person.inference import load_detection_model, detect_faces + +class DetectPersonSM(ActionSMBase): + def __init__(self, timeout=120., + image_topic='/cam3d/rgb/image_raw', + detection_model_path='', + max_recovery_attempts=1): + super(DetectPersonSM, self).__init__('DetectPerson', [], max_recovery_attempts) self.timeout = timeout + self.detection_model_path = detection_model_path self.bridge = CvBridge() - self.image_publisher = rospy.Publisher(image_topic, Image, queue_size=1) + self.image_publisher = rospy.Publisher(image_topic, Image, queue_size=1) + self.face_detection = None - # loading model - self.face_detection = load_detection_model(detection_model_path) - - def execute(self, userdata): - userdata.bounding_boxes = [] - self.input_image = self.convert_image(userdata.detect_person_goal.image) + def init(self): + try: + rospy.loginfo('[detect_person] Loading detection model %s', self.detection_model_path) + self.face_detection = load_detection_model(detection_model_path) + except: + rospy.logerr('[detect_person] Model %s could not be loaded', self.detection_model_path) + return FTSMTransitions.INITIALISED + + def running(self): + bounding_boxes = [] + number_of_faces = 0 + detection_successful = True + input_image = self.__convert_image(self.goal.image) try: - bgr_image = self.input_image + bgr_image = input_image gray_image = cv2.cvtColor(bgr_image, cv2.COLOR_BGR2GRAY) rgb_image = cv2.cvtColor(bgr_image, cv2.COLOR_BGR2RGB) faces = detect_faces(self.face_detection, gray_image) - userdata.number_of_faces = np.size(faces, 0) - except: - userdata.number_of_faces = 0 + number_of_faces = np.size(faces, 0) - if userdata.number_of_faces != 0: for face_coordinates in faces: bounding_box = FaceBoundingBox() - print type(face_coordinates) bounding_box.bounding_box_coordinates = face_coordinates.tolist() - userdata.bounding_boxes.append(bounding_box) + bounding_boxes.append(bounding_box) x, y, w, h = face_coordinates - rgb_cv2 = cv2.rectangle(rgb_image, (x, y), (x + w, y + h), (0,0,255), 2) + rgb_cv2 = cv2.rectangle(rgb_image, (x, y), (x + w, y + h), (0, 0, 255), 2) output_ros_image = self.bridge.cv2_to_imgmsg(rgb_image, 'bgr8') self.image_publisher.publish(output_ros_image) - return 'succeeded' - else: - return 'failed' + except: + detection_successful = False + + self.result = self.set_result(detection_successful, number_of_faces, bounding_boxes) + return FTSMTransitions.DONE - def convert_image(self, ros_image): + def __convert_image(self, ros_image): cv_image = self.bridge.imgmsg_to_cv2(ros_image, 'bgr8') return np.array(cv_image, dtype=np.uint8) -class SetActionLibResult(smach.State): - def __init__(self, result): - smach.State.__init__(self, outcomes=['succeeded'], - input_keys=['number_of_faces', 'bounding_boxes'], - output_keys=['detect_person_result','detect_person_feedback']) - self.result = result - - def execute(self, userdata): + def set_result(self, detection_successful, number_of_faces, bounding_boxes): result = DetectPersonResult() - result.success = self.result - result.number_of_faces = userdata.number_of_faces - result.bounding_boxes = userdata.bounding_boxes - userdata.detect_person_result = result - return 'succeeded' + result.success = detection_successful + result.number_of_faces = number_of_faces + result.bounding_boxes = bounding_boxes + return result diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/src/mdr_detect_person/skill.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/src/mdr_detect_person/skill.py deleted file mode 100644 index c98b07a2d..000000000 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_detect_person/ros/src/mdr_detect_person/skill.py +++ /dev/null @@ -1,38 +0,0 @@ -#!/usr/bin/env python -import rospy -import smach -from smach import StateMachine as sm -from mdr_detect_person.msg import DetectPersonAction -from mdr_detect_person.action_states import (SetupDetectPerson, DetectPerson, - SetActionLibResult) - - -class DetectPersonSkill(smach.StateMachine): - def __init__(self, timeout=10): - sm.__init__(self, - outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED', 'PREEMPTED'], - input_keys=['detect_person_goal'], - output_keys=['detect_person_feedback', - 'detect_person_result']) - - detection_model_path = rospy.get_param('~config_file', '') - image_topic = rospy.get_param('~image_topic', '/cam3d/rgb/image_raw') - - with self: - sm.add('SETUP_DETECT_PERSON', SetupDetectPerson(), - transitions={'succeeded': 'DETECT_PERSON', - 'failed': 'SETUP_DETECT_PERSON'}) - - sm.add('DETECT_PERSON', - DetectPerson(image_topic=image_topic, - detection_model_path=detection_model_path), - transitions={'succeeded': 'SET_ACTION_LIB_SUCCESS', - 'failed': 'SET_ACTION_LIB_FAILED'}) - - sm.add('SET_ACTION_LIB_FAILED', - SetActionLibResult(False), - transitions={'succeeded': 'OVERALL_FAILED'}) - - sm.add('SET_ACTION_LIB_SUCCESS', - SetActionLibResult(True), - transitions={'succeeded': 'OVERALL_SUCCESS'}) diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/CMakeLists.txt b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/CMakeLists.txt new file mode 100644 index 000000000..f22fecf15 --- /dev/null +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/CMakeLists.txt @@ -0,0 +1,57 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mdr_find_object_action) + +find_package(catkin REQUIRED COMPONENTS + rospy + roslint + genmsg + message_generation + actionlib + actionlib_msgs + mas_perception_libs + mas_perception_msgs + mas_knowledge_base + mdr_move_base_action + mdr_perceive_plane_action +) + +catkin_python_setup() +roslint_python() + +add_action_files(DIRECTORY ros/action + FILES + FindObject.action +) + +generate_messages( + DEPENDENCIES + actionlib_msgs + mas_perception_msgs +) + +catkin_package( + CATKIN_DEPENDS + rospy + actionlib + actionlib_msgs + message_runtime + mas_perception_libs + mas_perception_msgs + mas_knowledge_base + mdr_move_base_action + mdr_perceive_plane_action +) + +include_directories( + ${catkin_INCLUDE_DIRS} +) + +install(PROGRAMS + ros/scripts/find_object_action + ros/scripts/find_object_action_client_test + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/scripts +) + +install(DIRECTORY ros/launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +) diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/README.md b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/README.md new file mode 100644 index 000000000..bf31990e1 --- /dev/null +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/README.md @@ -0,0 +1,88 @@ +# ``mdr_find_object_action`` + +An action for finding objects in a familiar environment. The action includes an action server as well as an action client that interacts with ROSPlan. + +## Action definition + +### Goal constants: + +#### Goal types: + +* ``int32 NAMED_OBJECT=1`` +* ``int32 OBJECT_CATEGORY=2`` + +### Goal: + +* ``int32 goal_type``: `NAMED_OBJECT` or `OBJECT_CATEGORY` +* ``string object_name``: Name of an object if `goal_type` is `NAMED_OBJECT`; name of an object category if `goal_type` is `OBJECT_CATEGORY` + +### Result: + +* ``bool success``: Indicates whether the action was successfully completed +* ``mas_perception_msgs/Object obj``: The object (if found) +* ``string object_location``: The location of the object if the object was found +* ``string relation``: The relation of the found object and its location (such as "on" or "in") + +### Feedback: + +* ``string current_state`` +* ``string message`` + +## Directory structure + +``` +├── CMakeLists.txt +├── package.xml +├── README.md +├── setup.py +└── ros +    ├── action +    │   └── FindObject.action +    ├── launch +    │   ├── find_object.launch +    │   └── find_object_client.launch +    ├── scripts +    │   ├── find_object_action +    │   ├── find_object_client +    │   └── find_object_action_client_test +    └── src +    └── mdr_find_object_action +    ├── action_states.py +    └── __init__.py +``` + +## Launch file parameters + +### Action server + +The following arguments may be passed when launching the action server: + +* ``ontology_url``: URL of an ontology file (if a local ontology is loaded, this parameter should be set to the the absolute path of the ontology prefixed by `file://`) +* ``ontology_class_prefix``: Prefix of the entities in the ontology +* ``retry_count_on_failure``: Number of times to retry the action if the execution fails (default 0) +* ``timeout_s``: Seconds to wait for the action to be executed (default 120.0) + +### Action client + +The action client that interacts with ROSPlan inherits from the action client base class defined in ``mdr_rosplan_interface``. + +The following parameters need to be passed when launching the action client: +* ``action_name``: Name of the action as used in the planning domain +* ``server_name``: Name of the ``find_object`` action server +* ``action_timeout``: Maximum time (in seconds) that we are willing to wait for the action to be executed +* ``action_dispatch_topic``: Name of the topic at which the plan dispatcher sends action requests +* ``action_feedback_topic``: Name of the topic at which the action sends feedback to the plan dispatcher + +## Dependencies + +* ``mas_perception_libs`` +* ``mas_perception_msgs`` +* ``mas_knowledge_base`` +* ``mdr_move_base_action`` +* ``mdr_perceive_plane_action`` + +## Example usage + +1. Run the robot simulation: ``roslaunch mas__bringup_sim robot.launch`` +2. Run the action server: ``roslaunch mas__find_object_action find_object.launch`` +3. Run the client example: ``rosrun mdr_find_object_action find_object_action_client_test`` diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/package.xml b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/package.xml new file mode 100644 index 000000000..24057f5ae --- /dev/null +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/package.xml @@ -0,0 +1,37 @@ + + + mdr_find_object_action + 1.0.0 + An action for finding objects in a familiar environment + + Alex Mitrevski + MAS robotics + + GPLv3 + + catkin + genmsg + + rospy + roslint + message_generation + actionlib + actionlib_msgs + mas_perception_libs + mas_perception_msgs + mas_knowledge_base + mdr_move_base_action + mdr_perceive_plane_action + + rospy + actionlib + actionlib_msgs + message_runtime + mas_perception_libs + mas_perception_msgs + mas_knowledge_base + mdr_move_base_action + mdr_perceive_plane_action + + + diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/action/FindObject.action b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/action/FindObject.action new file mode 100644 index 000000000..288034db1 --- /dev/null +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/action/FindObject.action @@ -0,0 +1,16 @@ +int32 NAMED_OBJECT=1 +int32 OBJECT_CATEGORY=2 + +# goal definition +int32 goal_type +string object_name +--- +# result definition +bool success +mas_perception_msgs/Object obj +string object_location +string relation +--- +# feedback +string current_state +string message diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/launch/find_object.launch b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/launch/find_object.launch new file mode 100644 index 000000000..fd8ed0c7e --- /dev/null +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/launch/find_object.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/launch/find_object_client.launch b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/launch/find_object_client.launch new file mode 100644 index 000000000..b70002cb5 --- /dev/null +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/launch/find_object_client.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_action b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_action new file mode 100755 index 000000000..048aa6ad1 --- /dev/null +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_action @@ -0,0 +1,64 @@ +#!/usr/bin/env python +import rospy +import actionlib + +from mdr_find_object_action.msg import FindObjectAction +from mdr_find_object_action.action_states import FindObjectSM + +class FindObjectServer(object): + '''A server exposing an action for locating objects. + + The server expects the following parameters to be made available on the ROS parameter server: + * ontology_url: URL of a domestic ontology file + * ontology_class_prefix: Prefix of the classes in the ontology + * retry_count_on_failure: Number of times the search should be retried + if it fails the first time + * timeout_s: Timeout (in seconds) for the action execution + + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de + + ''' + def __init__(self): + ontology_url = rospy.get_param('~ontology_url', '') + ontology_class_prefix = rospy.get_param('~ontology_class_prefix', '') + retry_count_on_failure = int(rospy.get_param('~retry_count_on_failure', 0)) + timeout_s = float(rospy.get_param('~timeout_s', 120.)) + if not ontology_url: + rospy.logerr('[find_object] Ontology url not specified') + + if not ontology_class_prefix: + rospy.logerr('[find_object] Ontology class prefix not specified') + + rospy.loginfo('[find_object] Initialising state machine') + self.action_sm = FindObjectSM(ontology_url=ontology_url, + ontology_class_prefix=ontology_class_prefix, + number_of_retries=retry_count_on_failure, + timeout=timeout_s) + rospy.loginfo('[find_object] State machine initialised') + + self.action_server = actionlib.SimpleActionServer('find_object_server', + FindObjectAction, + self.execute, False) + self.action_server.start() + rospy.loginfo('find_object action server ready') + + def execute(self, goal): + rospy.loginfo('[find_object] Received an action request') + self.action_sm.goal = goal + self.action_sm.result = None + self.action_sm.execution_requested = True + while not self.action_sm.result: + rospy.sleep(0.05) + self.action_server.set_succeeded(self.action_sm.result) + +if __name__ == '__main__': + rospy.init_node('find_object_server') + find_object_server = FindObjectServer() + try: + find_object_server.action_sm.run() + while find_object_server.action_sm.is_running and not rospy.is_shutdown(): + rospy.spin() + except (KeyboardInterrupt, SystemExit): + print('{0} interrupted; exiting...'.format(find_object_server.action_sm.name)) + find_object_server.action_sm.stop() diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_action_client_test b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_action_client_test new file mode 100755 index 000000000..4b101d5de --- /dev/null +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_action_client_test @@ -0,0 +1,20 @@ +#! /usr/bin/env python +import rospy +import actionlib +from mdr_find_object_action.msg import FindObjectAction, FindObjectGoal + +if __name__ == '__main__': + rospy.init_node('find_object_client_test') + client = actionlib.SimpleActionClient('find_object_server', FindObjectAction) + client.wait_for_server() + goal = FindObjectGoal() + goal.goal_type = FindObjectGoal.NAMED_OBJECT + goal.object_name = 'CoffeeMug' + try: + timeout = 45.0 + rospy.loginfo('Sending action lib goal to find_object_server') + client.send_goal(goal) + client.wait_for_result(rospy.Duration.from_sec(int(timeout))) + rospy.loginfo(client.get_result()) + except: + pass diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_client b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_client new file mode 100755 index 000000000..253f49662 --- /dev/null +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/scripts/find_object_client @@ -0,0 +1,79 @@ +#!/usr/bin/env python +import rospy +import actionlib + +from mdr_rosplan_interface.action_client_base import ActionClientBase +from mdr_find_object_action.msg import FindObjectAction, FindObjectGoal + +class FindObjectClient(ActionClientBase): + '''Defines a client for an action for finding objects. + + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de + + ''' + def __init__(self): + super(FindObjectClient, self).__init__() + self.object_name = '' + while not rospy.is_shutdown(): + rospy.sleep(0.1) + + def call_action(self, msg): + # we only react to calls to this action + if self.action_name != msg.name.lower(): + return + + self.action_id = msg.action_id + + client = actionlib.SimpleActionClient(self.action_server_name, FindObjectAction) + client.wait_for_server() + goal = self.get_action_message(msg) + + # calling the actionlib server and waiting for the execution to end + rospy.loginfo('[FIND_OBJECT] Sending action lib goal to' + self.action_server_name) + client.send_goal(goal) + client.wait_for_result(rospy.Duration.from_sec(int(self.action_timeout))) + result = client.get_result() + + if result and result.success: + rospy.loginfo('[FIND_OBJECT] %s %s', result.object_location, result.relation) + self.update_knowledge_base(self.object_name, result.object_location, result.relation) + self.send_action_feedback(True) + else: + self.send_action_feedback(False) + + def get_action_message(self, rosplan_action_msg): + '''Reads the action parameters and uses them to initialise an actionlib message. + ''' + goal = FindObjectGoal() + for param in rosplan_action_msg.parameters: + if param.key == 'obj_name': + goal.goal_type = FindObjectGoal.NAMED_OBJECT + goal.object_name = param.value + self.object_name = param.value + elif param.key == 'obj_category': + goal.goal_type = FindObjectGoal.OBJECT_CATEGORY + goal.object_name = param.value + self.object_name = param.value # TODO: make the name unique + elif param.key == 'bot': + self.robot_name = param.value + return goal + + def update_knowledge_base(self, object_name, object_location, relation): + '''TODO: Decide what to do with the detected object; this is not so simple + since there are multiple cases that need to be considered: + * the object was already in the knowledge base + * the object's location was taken from the ontology and then verified; + in this case, we want to insert a relation and save the object + * the robot was searching for an object of a given category and found + a specific instance; this is similar to the previous case, but + we need to make sure that the name of the object is unique + ''' + pass + +if __name__ == '__main__': + rospy.init_node('find_object_client') + try: + FindObjectClient() + except rospy.ROSInterruptException: + pass diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/ros/src/mdr_demo_patrol/__init__.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/src/mdr_find_object_action/__init__.py similarity index 100% rename from mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/ros/src/mdr_demo_patrol/__init__.py rename to mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/src/mdr_find_object_action/__init__.py diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/src/mdr_find_object_action/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/src/mdr_find_object_action/action_states.py new file mode 100644 index 000000000..d4a67adbb --- /dev/null +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/ros/src/mdr_find_object_action/action_states.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python +import rospy +from pyftsm.ftsm import FTSMTransitions +from mas_execution.action_sm_base import ActionSMBase +from mas_knowledge_utils.domestic_ontology_interface import DomesticOntologyInterface +from mas_knowledge_base.domestic_kb_interface import DomesticKBInterface +from mdr_find_object_action.msg import FindObjectGoal, FindObjectResult + +class FindObjectSM(ActionSMBase): + def __init__(self, ontology_url, + ontology_class_prefix, + number_of_retries=0, + timeout=120., + max_recovery_attempts=1): + super(FindObjectSM, self).__init__('FindObject', [], max_recovery_attempts) + self.ontology_url = ontology_url + self.ontology_class_prefix = ontology_class_prefix + self.number_of_retries = number_of_retries + self.timeout = timeout + self.ontology_interface = None + self.kb_interface = None + + def init(self): + try: + rospy.loginfo('[find_object] Creating an interface client for ontology %s', self.ontology_url) + self.ontology_interface = DomesticOntologyInterface(self.ontology_url, + self.ontology_class_prefix) + except Exception as exc: + rospy.logerr('[find_object] Could not create an ontology interface client: %s', exc) + + try: + rospy.loginfo('[find_object] Creating a knowledge base interface client') + self.kb_interface = DomesticKBInterface() + except Exception as exc: + rospy.logerr('[find_object] Could not create a knowledge base interface client: %s', exc) + return FTSMTransitions.INITIALISED + + def running(self): + obj_name = None + obj_location = None + relation = None + if self.goal.goal_type == FindObjectGoal.NAMED_OBJECT: + obj_name = self.goal.object_name + location, predicate = self.kb_interface.get_object_location(obj_name) + if location: + rospy.loginfo('[find_object] Found %s %s %s', obj_name, predicate, location) + # TODO: verify that the object is still at that location + obj_location = location + relation = predicate + self.result = self.set_result(True, obj_location, relation) + return FTSMTransitions.DONE + + rospy.loginfo('[find_object] %s not found in the knowledge base; querying the ontology', obj_name) + location = self.ontology_interface.get_default_storing_location(obj_name=obj_name) + if location: + predicate = 'in' + rospy.loginfo('[find_object] %s is usually %s %s', obj_name, predicate, location) + # TODO: check if the object is at the default location + obj_location = location + relation = predicate + self.result = self.set_result(True, obj_location, relation) + return FTSMTransitions.DONE + + rospy.loginfo('[find_object] %s not found', obj_name) + obj_location = None + relation = None + self.result = self.set_result(True, obj_location, relation) + return FTSMTransitions.DONE + elif self.goal.goal_type == FindObjectGoal.OBJECT_CATEGORY: + obj_category = self.goal.object_name + category_objects = self.kb_interface.get_category_objects(obj_category) + if category_objects: + for obj_name in category_objects: + location, predicate = self.kb_interface.get_object_location(obj_name) + if location: + rospy.loginfo('[find_object] Found %s %s %s', obj_name, predicate, location) + # TODO: verify that the object is still at that location + obj_location = location + relation = predicate + self.result = self.set_result(True, obj_location, relation) + return FTSMTransitions.DONE + else: + rospy.loginfo('[find_object] The location of %s is unknown', obj_name) + else: + rospy.loginfo('[find_object] No object of category %s was found in the knowledge base; querying the ontology', obj_category) + + location = self.ontology_interface.get_default_storing_location(obj_category=obj_category) + if location: + predicate = 'in' + rospy.loginfo('[find_object] Objects of %s are usually %s %s', obj_category, predicate, location) + # TODO: check if an object of the desired category is at the default location + obj_location = location + relation = predicate + self.result = self.set_result(True, obj_location, relation) + return FTSMTransitions.DONE + + rospy.logerr('[find_object] Object of category %s could not be found', obj_category) + self.result = self.set_result(False, obj_location, relation) + return FTSMTransitions.DONE + + def set_result(self, success, obj_location, relation): + result = FindObjectResult() + result.success = success + result.object_location = obj_location + result.relation = relation + return result diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/setup.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/setup.py similarity index 54% rename from mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/setup.py rename to mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/setup.py index 0789b4ffa..d098f420c 100644 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/setup.py +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_find_object_action/setup.py @@ -4,8 +4,8 @@ from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( - packages=['mdr_demo_throw_table_objects'], - package_dir={'mdr_demo_throw_table_objects': 'ros/src/mdr_demo_throw_table_objects'} + packages=['mdr_find_object_action'], + package_dir={'mdr_find_object_action': 'ros/src/mdr_find_object_action'} ) setup(**d) diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/scripts/gender_recognition_action b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/scripts/gender_recognition_action index f21c97e84..6138922f3 100755 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/scripts/gender_recognition_action +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/scripts/gender_recognition_action @@ -1,34 +1,55 @@ #!/usr/bin/env python import rospy -import smach +import actionlib -from smach_ros import ActionServerWrapper, IntrospectionServer -from mdr_gender_recognition.skill import GenderRecognitionSkill from mdr_gender_recognition.msg import GenderRecognitionAction +from mdr_gender_recognition.action_states import RecognizeGenderSM +class RecognizeGenderServer(object): + '''A server exposing an action for recognizing the genders of previously detected people. + + The server expects the following parameters to be made available on the ROS parameter server: + * gender_model_path: Absolute path of a gender recognition model (default '') + * image_topic: Name of a camera image topic (default '/cam3d/rgb/image_raw') + + @author Alex Mitrevski, Octavio Arriaga + @contact aleksandar.mitrevski@h-brs.de + + ''' + def __init__(self): + gender_model_path = rospy.get_param('~gender_model_path', '') + image_topic = rospy.get_param('~image_topic', '/cam3d/rgb/image_raw') + labels = {0: 'woman', 1: 'man'} + image_size = (64, 64, 1) + + rospy.loginfo('[recognize_gender] Initialising state machine') + self.action_sm = RecognizeGenderSM(gender_model_path=gender_model_path, + image_topic=image_topic, + labels=labels, image_size=image_size) + rospy.loginfo('[recognize_gender] State machine initialised') + + self.action_server = actionlib.SimpleActionServer('recognize_gender_server', + GenderRecognitionAction, + self.execute, False) + self.action_server.start() + rospy.loginfo('recognize_gender action server ready') + + def execute(self, goal): + rospy.loginfo('[recognize_gender] Received an action request') + self.action_sm.goal = goal + self.action_sm.result = None + self.action_sm.execution_requested = True + while not self.action_sm.result: + rospy.sleep(0.05) + self.action_server.set_succeeded(self.action_sm.result) if __name__ == '__main__': - rospy.init_node('gender_recognition_server') - - # construct state machine - sm = GenderRecognitionSkill() - - # smach viewer - sis = IntrospectionServer('gender_recognition_smach_viewer', sm, - '/gender_recognition_SMACH_VIEWER') - sis.start() - - asw = ActionServerWrapper( - server_name='gender_recognition_server', - action_spec=GenderRecognitionAction, - wrapped_container=sm, - succeeded_outcomes=['OVERALL_SUCCESS'], - aborted_outcomes=['OVERALL_FAILED'], - preempted_outcomes=['PREEMPTED'], - goal_key='gender_recognition_goal', - feedback_key='gender_recognition_feedback', - result_key='gender_recognition_result') - - # Run the server in a background thread - asw.run_server() - rospy.spin() + rospy.init_node('recognize_gender_server') + recognize_gender_server = RecognizeGenderServer() + try: + recognize_gender_server.action_sm.run() + while recognize_gender_server.action_sm.is_running and not rospy.is_shutdown(): + rospy.spin() + except (KeyboardInterrupt, SystemExit): + print('{0} interrupted; exiting...'.format(recognize_gender_server.action_sm.name)) + recognize_gender_server.action_sm.stop() diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/src/mdr_gender_recognition/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/src/mdr_gender_recognition/action_states.py index 10cf2d016..d0fb1e5dd 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/src/mdr_gender_recognition/action_states.py +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/src/mdr_gender_recognition/action_states.py @@ -1,111 +1,89 @@ -#!/usr/bin/python - -import rospy -import smach -import smach_ros -import actionlib -from sensor_msgs.msg import Image - import numpy as np import cv2 import tensorflow as tf from keras.models import load_model from cv_bridge import CvBridge, CvBridgeError -from mdr_gender_recognition.msg import GenderRecognitionFeedback, GenderRecognitionResult - - -class SetupGenderRecognition(smach.State): - def __init__(self): - smach.State.__init__(self, outcomes=['succeeded', 'failed'], - input_keys=['gender_recognition_goal'], - output_keys=['gender_recognition_feedback', 'gender_recognition_result']) - - def execute(self, userdata): - feedback = GenderRecognitionFeedback() - feedback.current_state = 'RECOGNIZE_GENDERS' - feedback.message = '[gender_recognition] Recognizing gender' - userdata.gender_recognition_feedback = feedback - return 'succeeded' +import rospy +from sensor_msgs.msg import Image +from pyftsm.ftsm import FTSMTransitions +from mas_execution.action_sm_base import ActionSMBase +from mdr_gender_recognition.msg import GenderRecognitionFeedback, GenderRecognitionResult -class RecognizeGenders(smach.State): +class RecognizeGenderSM(ActionSMBase): def __init__(self, timeout=120.0, image_topic='/cam3d/rgb/image_raw', - gender_model_path=None, labels=dict(), image_size=(0, 0, 0)): - smach.State.__init__(self, input_keys=['gender_recognition_goal', 'genders'], - output_keys=['gender_recognition_feedback', 'genders'], - outcomes=['succeeded', 'failed']) + gender_model_path=None, labels=None, image_size=(0, 0, 0), + max_recovery_attempts=1): + super(RecognizeGenderSM, self).__init__('RecognizeGender', [], max_recovery_attempts) self.timeout = timeout + self.gender_model_path = gender_model_path self.labels = labels self.image_size = image_size - self.nop = np.array([None]) self.image_publisher = rospy.Publisher(image_topic, Image, queue_size=1) self.bridge = CvBridge() + self.gender_model = None + self.computation_graph = None + def init(self): try: - # loading model + rospy.loginfo('[recognize_gender] Loading model %s', self.gender_model_path) self.gender_model = load_model(gender_model_path) - print('Model ' + gender_model_path + ' loaded successfully') # the following two lines are necessary for avoiding https://github.com/keras-team/keras/issues/2397 self.gender_model._make_predict_function() self.computation_graph = tf.get_default_graph() except: - print('Failed to load model ', gender_model_path) + rospy.logerr('[recognize_gender] Failed to load model %s', self.gender_model_path) + return FTSMTransitions.INITIALISED - def execute(self, userdata): - number_of_faces = userdata.gender_recognition_goal.number_of_faces - bounding_boxes = userdata.gender_recognition_goal.bounding_boxes - userdata.genders = list() + def running(self): + number_of_faces = self.goal.number_of_faces + bounding_boxes = self.goal.bounding_boxes + genders = [] - rgb_image = self.ros2cv(userdata.gender_recognition_goal.image) - gray_image = self.rgb2gray(rgb_image) + rospy.loginfo('[recognize_gender] Recognizing genders') + rgb_image = self.__ros2cv(self.goal.image) + gray_image = self.__rgb2gray(rgb_image) for face in bounding_boxes: x, y, w, h = face.bounding_box_coordinates face = gray_image[y: (y + h), x: (x + w)] # check if it is not rgb face = cv2.resize(face, self.image_size[0:2]) face = np.expand_dims(face, 0) face = np.expand_dims(face, -1) - face = self.preprocess_image(face) - recognized_gender = self.recognize_gender(face) - userdata.genders.append(recognized_gender) + face = self.__preprocess_image(face) + recognized_gender = self.__recognize_gender(face) + genders.append(recognized_gender) rgb_cv2 = cv2.rectangle(rgb_image, (x, y), (x + w, y + h), (0, 0, 255), 2) cv2.putText(rgb_image, recognized_gender, (x, y - 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0), 1, cv2.LINE_AA) output_ros_image = self.bridge.cv2_to_imgmsg(rgb_image, 'bgr8') self.image_publisher.publish(output_ros_image) - return 'succeeded' - def preprocess_image(self, image): + self.result = self.set_result(True, genders) + return FTSMTransitions.DONE + + def set_result(self, success, genders): + result = GenderRecognitionResult() + result.success = success + result.genders = genders + return result + + def __preprocess_image(self, image): image = image / 255.0 return image - def recognize_gender(self, face): + def __recognize_gender(self, face): label = -1 with self.computation_graph.as_default(): class_predictions = self.gender_model.predict(face) label = self.labels[np.argmax(class_predictions)] return label - def ros2cv(self, ros_image): + def __ros2cv(self, ros_image): cv_image = self.bridge.imgmsg_to_cv2(ros_image, 'bgr8') return np.array(cv_image, dtype=np.uint8) - def rgb2gray(self, image): + def __rgb2gray(self, image): gray_image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) return gray_image - - -class SetActionLibResult(smach.State): - def __init__(self, result): - smach.State.__init__(self, outcomes=['succeeded'], - input_keys=['gender_recognition_goal', 'genders'], - output_keys=['gender_recognition_feedback', 'gender_recognition_result']) - self.result = result - - def execute(self, userdata): - result = GenderRecognitionResult() - result.success = self.result - result.genders = userdata.genders - userdata.gender_recognition_result = result - return 'succeeded' diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/src/mdr_gender_recognition/skill.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/src/mdr_gender_recognition/skill.py deleted file mode 100644 index 80a1aa5b9..000000000 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_gender_recognition/ros/src/mdr_gender_recognition/skill.py +++ /dev/null @@ -1,44 +0,0 @@ -#!/usr/bin/env python -import rospy -import smach -from smach import StateMachine as sm - -from mdr_gender_recognition.msg import GenderRecognitionAction -from mdr_gender_recognition.action_states import (SetupGenderRecognition, - RecognizeGenders, - SetActionLibResult) - - -class GenderRecognitionSkill(smach.StateMachine): - def __init__(self, timeout=10): - sm.__init__(self, - outcomes=['OVERALL_SUCCESS', - 'OVERALL_FAILED', 'PREEMPTED'], - input_keys=['gender_recognition_goal'], - output_keys=['gender_recognition_feedback', - 'gender_recognition_result']) - - gender_model_path = rospy.get_param('~gender_model_path', '') - image_topic = rospy.get_param('~image_topic', '/cam3d/rgb/image_raw') - labels = {0: 'woman', 1: 'man'} - image_size = (64, 64, 1) - - with self: - sm.add('SETUP_GENDER_RECOGNITION', SetupGenderRecognition(), - transitions={'succeeded': 'RECOGNIZE_GENDERS', - 'failed': 'SETUP_GENDER_RECOGNITION'}) - - sm.add('RECOGNIZE_GENDERS', - RecognizeGenders(gender_model_path=gender_model_path, - image_topic=image_topic, - labels=labels, image_size=image_size), - transitions={'succeeded': 'SET_ACTION_LIB_SUCCESS', - 'failed': 'SET_ACTION_LIB_FAILED'}) - - sm.add('SET_ACTION_LIB_FAILED', - SetActionLibResult(False), - transitions={'succeeded': 'OVERALL_FAILED'}) - - sm.add('SET_ACTION_LIB_SUCCESS', - SetActionLibResult(True), - transitions={'succeeded': 'OVERALL_SUCCESS'}) diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/CMakeLists.txt b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/CMakeLists.txt index 7157b9353..8e2423e92 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/CMakeLists.txt +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/CMakeLists.txt @@ -9,7 +9,7 @@ find_package(catkin REQUIRED COMPONENTS roslint rospy roslaunch - mcr_perception_msgs + mas_perception_msgs mas_perception_libs ) @@ -23,7 +23,7 @@ add_action_files(DIRECTORY ros/action generate_messages( DEPENDENCIES actionlib_msgs - mcr_perception_msgs + mas_perception_msgs ) catkin_package( diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/README.md b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/README.md index c3bef11e1..eb90fe9d7 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/README.md +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/README.md @@ -7,17 +7,15 @@ interacts with ROSPlan. ### Goal: -* ``string plane_config``: Name of the configuration set for setting up the dynamic parameter server. Each plane should - have a set of configurations, and all configuration sets should be listed in - ``config/perceive_plane_configurations.yaml``. This is necessary for configuring the scene segmentation node in - ``mas_common_robotics`` +* ``string plane_config``: Name of the plane configuration to be perceived. + TODO(minhnh): confirm that this can be removed. * ``string plane_frame_prefix``: prefix to the plane names which will be written to the ``Plane`` objects in ``action_states.DetectObjects`` ### Result: * ``bool success`` -* ``mcr_perception_msgs/PlaneList recognized_planes`` +* ``mas_perception_msgs/PlaneList recognized_planes`` ### Feedback: @@ -33,8 +31,6 @@ interacts with ROSPlan. ├── ros │   ├── action │   │   └── PerceivePlane.action -│   ├── config -│   │   └── perceive_plane_configurations.yaml │   ├── launch │   │   ├── perceive_plane_client.launch │   │   ├── perceive_plane.launch @@ -58,9 +54,9 @@ The following arguments may be passed when launching the action server: * ``target_frame``: name of the reference frame the object and plane poses will be transformed to (default: '/base_link') * ``detection_action_name``: name of the action server used for object detection. - Action file: `mcr_perception_msgs/DetectScene.action`. + Action file: `mas_perception_msgs/DetectScene.action`. * ``recognition_service_name``: the name of the image recognition service for classifying object. - Service file: `mcr_perception_msgs/ImageRecognition.srv`. + Service file: `mas_perception_msgs/RecognizeImage.srv`. * ``recognition_model_name``: the name of the image classification model located in the `mdr_object_recognition` package. * ``preprocess_input_module``: the name of the module containing the image preprocessing function to be executed on @@ -75,25 +71,21 @@ The following parameters need to be passed when launching the action client: * ``action_name``: Name of the action as used in the planning domain * ``server_name``: Name of the ``perceive_plane`` action server * ``action_timeout``: Maximum time (in seconds) that we are willing to wait for the action to be executed -* ``clear_plane_memory``: If set to true, all objects previously seen on a plane will be deleted from the knowledge base before new objects are added to it (default false) +* ``clear_plane_memory``: If set to true, all objects previously seen on a plane will be deleted from the + knowledge base before new objects are added to it (default false) * ``action_dispatch_topic``: Name of the topic at which the plan dispatcher sends action requests * ``action_feedback_topic``: Name of the topic at which the action sends feedback to the plan dispatcher -* ``knowledge_update_service``: Name of a service used for updating the planning problem as the world changes -* ``attribute_fetching_service``: Name of a service used for retrieving attributes representing the current knowledge -about the world ## Dependencies -* ``mdr_object_recognition_mean_circle`` * ``mas_perception_libs`` * ``mdr_object_recognition`` -* ``mcr_dynamic_reconfigure_client`` -* ``mcr_perception_msgs`` +* ``mas_perception_msgs`` ## Example usage 1. Change the ``service_module`` and ``service_class_name`` value to the desired extended service proxy. By default -a test service proxy of type ``std_srv/Empty`` is loaded. + a test service proxy of type ``std_srv/Empty`` is loaded. 2. Run the action server: ``roslaunch mdr_perceive_plane_action perceive_plane.launch``. -The ``perceive_plane_test.launch`` file will create a server of type ``std_srv/Empty`` for testing. + The ``perceive_plane_test.launch`` file will create a server of type ``std_srv/Empty`` for testing. 3. Run the client example: ``rosrun mdr_perceive_plane_action perceive_plane_client_test`` diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/package.xml b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/package.xml index 215b5ec63..da933b2f4 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/package.xml +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/package.xml @@ -18,7 +18,7 @@ genmsg actionlib - mcr_perception_msgs + mas_perception_msgs mas_perception_libs actionlib_msgs message_generation @@ -30,9 +30,7 @@ actionlib actionlib_msgs message_runtime - mcr_perception_msgs - mcr_states - mcr_dynamic_reconfigure_client + mas_perception_msgs mdr_object_recognition diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/action/PerceivePlane.action b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/action/PerceivePlane.action index af988b4df..d13047aca 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/action/PerceivePlane.action +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/action/PerceivePlane.action @@ -4,7 +4,7 @@ string plane_frame_prefix --- # result definition bool success -mcr_perception_msgs/PlaneList recognized_planes +mas_perception_msgs/PlaneList recognized_planes --- # feedback string current_state diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/config/perceive_plane_configurations.yaml b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/config/perceive_plane_configurations.yaml deleted file mode 100644 index b9c0df58f..000000000 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/config/perceive_plane_configurations.yaml +++ /dev/null @@ -1,39 +0,0 @@ -# configuration_name: -# node_name: -# param_name: param_value -# param_name: param_value -# node_name: -# param_name: param_value -# param_name: param_value -# configuration_name: -# node_name: -# param_name: param_value -# param_name: param_value -# node_name: -# param_name: param_value -# param_name: param_value - -shelf_1: - /mcr_perception/voxel_filter: - filter_limit_min: 0.3 - filter_limit_max: 0.6 -shelf_2: - /mcr_perception/voxel_filter: - filter_limit_min: 0.3 - filter_limit_max: 0.6 -shelf_3: - /mcr_perception/voxel_filter: - filter_limit_min: 0.6 - filter_limit_max: 0.9 -shelf_4: - /mcr_perception/voxel_filter: - filter_limit_min: 0.9 - filter_limit_max: 1.3 -shelf_5: - /mcr_perception/voxel_filter: - filter_limit_min: 1.2 - filter_limit_max: 1.8 -table: - /mcr_perception/voxel_filter: - filter_limit_min: 0.6 - filter_limit_max: 1.5 diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/launch/perceive_plane_client.launch b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/launch/perceive_plane_client.launch index 5dd8e04b0..f8ef50e31 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/launch/perceive_plane_client.launch +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/launch/perceive_plane_client.launch @@ -12,7 +12,5 @@ - - diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_action b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_action index 101ed8fbe..594d4be7d 100755 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_action +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_action @@ -1,76 +1,65 @@ #!/usr/bin/env python import rospy -import smach -from smach_ros import ActionServerWrapper, IntrospectionServer -from mas_perception_libs import Constant -from mdr_perceive_plane_action.msg import PerceivePlaneAction -from mdr_perceive_plane_action.action_states import SetActionLibResult, DetectObjects, RecognizeObjects - - -class PerceivePlaneSkill(smach.StateMachine): - def __init__(self, detection_action_name, target_frame, recog_service_name, recog_model_name, - preprocess_input_module): - smach.StateMachine.__init__(self, outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED', 'PREEMPTED'], - input_keys=['perceive_plane_goal'], - output_keys=['perceive_plane_feedback', 'perceive_plane_result']) - - with self: - smach.StateMachine.add('DETECT_OBJECTS', - DetectObjects(detection_action_name, target_frame), - transitions={Constant.SUCCESS: 'RECOGNIZE_OBJECTS', - Constant.TIMEOUT: 'SET_ACTION_LIB_FAILURE', - Constant.FAILURE: 'SET_ACTION_LIB_FAILURE'}) - - smach.StateMachine.add('RECOGNIZE_OBJECTS', RecognizeObjects(recog_service_name, recog_model_name, - preprocess_input_module), - transitions={Constant.SUCCESS: 'SET_ACTION_LIB_SUCCESS', - Constant.FAILURE: 'SET_ACTION_LIB_FAILURE'}) - - smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), - transitions={Constant.SUCCESS: 'OVERALL_SUCCESS'}) - - smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False), - transitions={Constant.SUCCESS: 'OVERALL_FAILED'}) - - -def main(): - rospy.init_node('~perceive_plane_server') - # detection - detection_action_name = rospy.get_param('~detection_action_name', '') - if not detection_action_name: - raise RuntimeError('no detection action server specified') - - # recognition - recognition_service_name = rospy.get_param('~recognition_service_name', '/mdr_perception/recognize_image') - recognition_model_name = rospy.get_param('~recognition_model_name', 'athome_xception_v1') - preprocess_input_module = rospy.get_param('~preprocess_input_module', None) - - target_frame = rospy.get_param("~target_frame") - - # Construct state machine - sm = PerceivePlaneSkill(detection_action_name, target_frame, recognition_service_name, recognition_model_name, - preprocess_input_module) - - # smach viewer - sis = IntrospectionServer('perceive_plane_smach_viewer', sm, '/PERCEIVE_PLANE_SMACH_VIEWER') - sis.start() - - # Construct action server wrapper - asw = ActionServerWrapper( - server_name='perceive_plane_server', - action_spec=PerceivePlaneAction, - wrapped_container=sm, - succeeded_outcomes=['OVERALL_SUCCESS'], - aborted_outcomes=['OVERALL_FAILED'], - preempted_outcomes=['PREEMPTED'], - goal_key='perceive_plane_goal', - feedback_key='perceive_plane_feedback', - result_key='perceive_plane_result') - - # Run the server in a background thread - asw.run_server() - rospy.spin() +import actionlib +from mdr_perceive_plane_action.msg import PerceivePlaneAction +from mdr_perceive_plane_action.action_states import PerceivePlaneSM + +class PerceivePlaneServer(object): + '''A server exposing an action for detecting objects on a plane. + + The server expects the following parameters to be made available on the ROS parameter server: + * detection_action_name: Name of an action server used for object detection + * recognition_service_name: Name of an image recognition service for classifying object + * recognition_model_name: Name of an image classification model + * preprocess_input_module: Name of a module containing an image preprocessing function + to be executed on images before inference + * target_frame: Name of the reference frame the object and plane poses will be + transformed to (default: '/base_link') + + @author Minh Nguyen, Alex Mitrevski + @contact minh.nguyen@smail.inf.h-brs.de, aleksandar.mitrevski@h-brs.de + + ''' + def __init__(self): + detection_action_name = rospy.get_param('~detection_action_name', '/base_link') + if not detection_action_name: + raise RuntimeError('[perceive_plane] no detection action server specified') + recognition_service_name = rospy.get_param('~recognition_service_name', '/mdr_perception/recognize_image') + recognition_model_name = rospy.get_param('~recognition_model_name', 'athome_xception_v1') + preprocess_input_module = rospy.get_param('~preprocess_input_module', None) + target_frame = rospy.get_param('~target_frame', '') + + rospy.loginfo('[perceive_plane] Initialising state machine') + self.action_sm = PerceivePlaneSM(detection_service_proxy=detection_action_name, + recog_service_name=recognition_service_name, + recog_model_name=recognition_model_name, + preprocess_input_module=preprocess_input_module, + target_frame=target_frame) + rospy.loginfo('[perceive_plane] State machine initialised') + + self.action_server = actionlib.SimpleActionServer('perceive_plane_server', + PerceivePlaneAction, + self.execute, False) + self.action_server.start() + rospy.loginfo('perceive_plane action server ready') + + def execute(self, goal): + rospy.loginfo('[perceive_plane] Received an action request') + self.action_sm.goal = goal + self.action_sm.result = None + self.action_sm.execution_requested = True + while not self.action_sm.result: + rospy.sleep(0.05) + self.action_server.set_succeeded(self.action_sm.result) if __name__ == '__main__': - main() + rospy.init_node('perceive_plane_server') + perceive_plane_server = PerceivePlaneServer() + try: + perceive_plane_server.action_sm.run() + while perceive_plane_server.action_sm.is_running and not rospy.is_shutdown(): + rospy.spin() + except (KeyboardInterrupt, SystemExit): + print('{0} interrupted; exiting...'.format(perceive_plane_server.action_sm.name)) + perceive_plane_server.action_sm.stop() diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_client b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_client index bf1ef3fac..1ef3a8264 100755 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_client +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/scripts/perceive_plane_client @@ -2,13 +2,18 @@ import rospy import actionlib +import rosplan_dispatch_msgs.msg as plan_dispatch_msgs +from mas_perception_msgs.msg import Plane, Object from mdr_rosplan_interface.action_client_base import ActionClientBase from mdr_perceive_plane_action.msg import PerceivePlaneAction, PerceivePlaneGoal -import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import rosplan_knowledge_msgs.srv as rosplan_srvs -import diagnostic_msgs.msg as diag_msgs class PerceivePlaneClient(ActionClientBase): + '''Defines a client for a plane scanning action. + + @author Minh Nguyen, Alex Mitrevski + @contact minh.nguyen@smail.inf.h-brs.de, aleksandar.mitrevski@h-brs.de + + ''' def __init__(self): super(PerceivePlaneClient, self).__init__() self.clear_plane_memory = rospy.get_param('~clear_plane_memory', False) @@ -53,119 +58,58 @@ class PerceivePlaneClient(ActionClientBase): return goal def update_knowledge_base(self, plane_prefix, plane_list): + '''Updates the knowledge base with the following facts: + * each of the detected planes is now explored, so it's not unexplored anymore + * each object on each of the detected plane is on the plane and has + the recognised category + ''' + facts_to_add = [] + facts_to_remove = [] + objects_to_add = [] for i, plane in enumerate(plane_list.planes): if self.clear_plane_memory: self.__delete_plane_from_kb(plane.name) - # we remove the fact that the plane is unexplored from the knowledge base - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 2 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'unexplored' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'plane' - arg_msg.value = plane.name - request.knowledge.values.append(arg_msg) + facts_to_add.append(('explored', [('plane', plane.name)])) + facts_to_remove.append(('unexplored', [('plane', plane.name)])) + objects_to_add.append((plane.name, plane)) - self.knowledge_update_client(request) - - # we add the fact that the plane is explored to the knowledge base - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 0 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'explored' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'plane' - arg_msg.value = plane.name - request.knowledge.values.append(arg_msg) - - self.knowledge_update_client(request) - self.msg_store_client.insert_named(plane.name, plane) - - # we add the detected objects to the knowledge base + # we update the list of facts and objects to add with the detected objects for obj in plane.object_list.objects: # we set a unique name for each object obj_name = obj.name + '_' + str(i) obj.name = obj_name - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 0 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'on' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'obj' - arg_msg.value = obj_name - request.knowledge.values.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'plane' - arg_msg.value = plane.name - request.knowledge.values.append(arg_msg) + facts_to_add.append(('on', [('obj', obj_name), ('plane', plane.name)])) + facts_to_add.append(('object_category', [('obj', obj_name), ('cat', obj.category)])) + objects_to_add.append((obj_name, obj)) - self.knowledge_update_client(request) - - # we add the object category to the knowledge base - request = rosplan_srvs.KnowledgeUpdateServiceRequest() - request.update_type = 0 - request.knowledge.knowledge_type = 1 - request.knowledge.attribute_name = 'object_category' - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'obj' - arg_msg.value = obj_name - request.knowledge.values.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'cat' - arg_msg.value = obj.category - request.knowledge.values.append(arg_msg) - - self.knowledge_update_client(request) - - # we add the object instance to the world model - self.msg_store_client.insert_named(obj_name, obj) + # we finally update the knowledge base + self.kb_interface.update_kb(facts_to_add, facts_to_remove) + self.kb_interface.insert_objects(objects_to_add) def __delete_plane_from_kb(self, plane_name): - '''Removes the plane with the given name and all objects on it from the message store - - Keyword arguments: - plane_name -- string indicating the name of a plane - - ''' - # we delete the plane from the knowledge base - self.msg_store_client.delete_named(plane_name) - - # we delete all objects on the plane from the knowledge base - plane_objects = self.__get_plane_objects(plane_name) - for obj in plane_objects: - self.msg_store_client.delete_named(obj) - - def __get_plane_objects(self, plane_name): - '''Returns a list with the names of all objects on the plane with the given name + '''Removes the plane with the given name and all objects on it + from the knowledge base. Keyword arguments: plane_name -- string indicating the name of a plane ''' - plane_objects = list() - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = 'on' - result = self.attribute_fetching_client(request) - for item in result.attributes: - object_on_desired_surface = False - object_name = '' - if not item.is_negative: - for param in item.values: - if param.key == 'plane' and param.value == plane_name: - object_on_desired_surface = True - elif param.key == 'obj': - object_name = param.value - if object_on_desired_surface: - plane_objects.append(object_name) - return plane_objects + facts_to_remove = [('explored', [('plane', plane_name)])] + objects_to_remove = [(plane_name, Plane._type)] + + # we update the lists of facts and objects to remove + # with the objects that on the plane + plane_objects = self.kb_interface.get_surface_object_names(plane_name) + for obj_name in plane_objects: + fact = ('on', [('obj', obj_name), ('plane', plane_name)]) + facts_to_remove.append(fact) + objects_to_remove.append((obj_name, Object._type)) + + # we finally remove the facts and objects from the knowledge base + self.kb_interface.remove_facts(facts_to_remove) + self.kb_interface.remove_objects(objects_to_remove) if __name__ == '__main__': rospy.init_node('perceive_plane_client') diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/src/mdr_perceive_plane_action/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/src/mdr_perceive_plane_action/action_states.py index 06c33a72b..5d5ac487e 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/src/mdr_perceive_plane_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_plane_action/ros/src/mdr_perceive_plane_action/action_states.py @@ -1,38 +1,36 @@ #!/usr/bin/python import rospy -import smach -import std_msgs.msg import sensor_msgs.msg +from pyftsm.ftsm import FTSMTransitions +from mas_execution.action_sm_base import ActionSMBase +from mas_perception_msgs.msg import PlaneList +from mas_perception_libs import ObjectDetector, RecognizeImageServiceProxy from mdr_perceive_plane_action.msg import PerceivePlaneResult, PerceivePlaneFeedback -from mcr_perception_msgs.msg import PlaneList -from mas_perception_libs import ObjectDetector, Constant, ImageRecognitionServiceProxy - -class DetectObjects(smach.State): - def __init__(self, detection_service_proxy, target_frame=None, timeout_duration=1): - smach.State.__init__(self, outcomes=[Constant.SUCCESS, Constant.FAILURE, Constant.TIMEOUT], - input_keys=['perceive_plane_goal', 'perceive_plane_feedback'], - output_keys=['detected_planes', 'perceive_plane_feedback']) +class PerceivePlaneSM(ActionSMBase): + def __init__(self, detection_service_proxy, + recog_service_name, + recog_model_name, + preprocess_input_module, + target_frame=None, + timeout_duration=1, + max_recovery_attempts=1): + super(PerceivePlaneSM, self).__init__('PerceivePlane', [], max_recovery_attempts) self._detector = ObjectDetector(detection_service_proxy) + self._recog_service_proxy = RecognizeImageServiceProxy(recog_service_name, recog_model_name, + preprocess_input_module) + self._image_pub = rospy.Publisher('/first_recognized_image', + sensor_msgs.msg.Image, + queue_size=1) self._timeout_duration = timeout_duration - self._detecting_done = False self._target_frame = target_frame - - def _detection_cb(self): - self._detecting_done = True - - def execute(self, ud): - if 'perceive_plane_feedback' not in ud: - ud.perceive_plane_feedback = PerceivePlaneFeedback() - ud.perceive_plane_feedback.current_state = 'detect_objects' - self._detecting_done = False - ud.detected_planes = None - if 'perceive_plane_goal' not in ud: - rospy.logerr('no goal passed into DetectObjects state') - return Constant.FAILURE - self._detector.start_detect_objects(ud.perceive_plane_goal.plane_frame_prefix, self._detection_cb, + def running(self): + detected_planes = None + self._detecting_done = False + self._detector.start_detect_objects(self.goal.plane_frame_prefix, + self._detection_cb, self._target_frame) timeout = rospy.Duration.from_sec(self._timeout_duration) @@ -41,128 +39,50 @@ def execute(self, ud): while (rospy.Time.now() - start_time) < timeout: if self._detecting_done: if self._detector.plane_list is None: - return Constant.FAILURE - - ud.detected_planes = self._detector.plane_list - return Constant.SUCCESS + rospy.logerr('[perceive_plane] No planes found') + self.result = self.set_result(False, detected_planes) + return FTSMTransitions.DONE + detected_planes = self._detector.plane_list rate.sleep() - return Constant.TIMEOUT - - -class RecognizeObjects(smach.State): - def __init__(self, recog_service_name, recog_model_name, preprocess_input_module): - smach.State.__init__(self, outcomes=[Constant.SUCCESS, Constant.FAILURE], - input_keys=['detected_planes', 'perceive_plane_feedback'], - output_keys=['recognized_planes', 'perceive_plane_feedback']) - - self._service_proxy = ImageRecognitionServiceProxy(recog_service_name, recog_model_name, - preprocess_input_module) - self._image_pub = rospy.Publisher('/first_recognized_image', sensor_msgs.msg.Image, queue_size=1) + if not self._detecting_done: + rospy.logerr('[perceive_plane] A plane could not be found within the alloted time') + self.result = self.set_result(False, detected_planes) + return FTSMTransitions.DONE - def execute(self, ud): - if 'perceive_plane_feedback' not in ud: - ud.perceive_plane_feedback = PerceivePlaneFeedback() - ud.perceive_plane_feedback.current_state = 'recognize_objects' - - if 'detected_planes' not in ud: - ud.recognized_planes = None - return Constant.FAILURE - - for plane in ud.detected_planes.planes: + rospy.loginfo('[perceive_plane] Found %d objects', len(detected_planes.planes)) + for plane in detected_planes.planes: image_messages = [] for obj in plane.object_list.objects: image_messages.append(obj.rgb_image) - indices, classes, probs = self._service_proxy.classify_image_messages(image_messages) + indices, classes, probs = self._recog_service_proxy.classify_image_messages(image_messages) # TODO: debug output if len(indices) > 0: obj_index = indices[0] self._image_pub.publish(image_messages[obj_index]) - rospy.loginfo('first object found: {0} (prob: {1})'.format(classes[obj_index], probs[obj_index])) + rospy.loginfo('[perceive_plane] first object found: {0} (prob: {1})'.format(classes[obj_index], + probs[obj_index])) else: - rospy.logwarn('no object recognized for plane ' + plane.name) + rospy.logwarn('[perceive_plane] no objects recognized for plane %s', plane.name) for i in indices: plane.object_list.objects[i].name = classes[i] # TODO: handle categories plane.object_list.objects[i].category = classes[i] - ud.recognized_planes = ud.detected_planes - return Constant.SUCCESS - + self.result = self.set_result(True, detected_planes) + return FTSMTransitions.DONE -class SetupPlaneConfig(smach.State): - def __init__(self, sleep_duration=1): - smach.State.__init__(self, outcomes=[Constant.SUCCESS, Constant.WAITING, Constant.FAILURE, Constant.TIMEOUT], - input_keys=['perceive_plane_goal'], - output_keys=['perceive_plane_feedback']) - self.sleep_duration = sleep_duration - - self.config_name_pub = rospy.Publisher("/mcr_common/dynamic_reconfigure_client/configuration_name", - std_msgs.msg.String, queue_size=1) - self.event_in_pub = rospy.Publisher("/mcr_common/dynamic_reconfigure_client/event_in", - std_msgs.msg.String, queue_size=1) - self.event_out_sub = rospy.Subscriber("/mcr_common/dynamic_reconfigure_client/event_out", - std_msgs.msg.String, self.event_cb) - self.event = None - - def event_cb(self, msg): - self.event = msg.data - - def configure_plane(self, config_name): - """ - :param config_name: configurations name listed in - config/perceive_plane_configurations.yaml - """ - self.event = None - self.config_name_pub.publish(config_name) - self.event_in_pub.publish("e_start") - - timeout = rospy.Duration.from_sec(1.0) - rate = rospy.Rate(10) - start_time = rospy.Time.now() - while (rospy.Time.now() - start_time) < timeout: - if self.event: - if self.event == Constant.E_SUCCESS: - return Constant.SUCCESS - return Constant.FAILURE - rate.sleep() - return Constant.TIMEOUT - - def execute(self, ud): - feedback = PerceivePlaneFeedback() - feedback.current_state = 'setup_plane_config' - if 'perceive_plane_goal' in ud: - feedback.message = '[perceive_plane] received plane config goal ' +\ - ud.perceive_plane_goal.plane_config - ud.perceive_plane_feedback = feedback - return self.configure_plane(ud.perceive_plane_goal.plane_config) - - feedback.message = '[perceive_plane] waiting for plane config goal' - ud.perceive_plane_feedback = feedback - rospy.sleep(self.sleep_duration) - return Constant.WAITING - - -class SetActionLibResult(smach.State): - def __init__(self, result): - smach.State.__init__(self, outcomes=[Constant.SUCCESS], - input_keys=['perceive_plane_goal', 'recognized_planes', 'perceive_plane_feedback'], - output_keys=['perceive_plane_feedback', 'perceive_plane_result']) - self.result = result - - def execute(self, ud): - if 'perceive_plane_feedback' not in ud: - ud.perceive_plane_feedback = PerceivePlaneFeedback() - ud.perceive_plane_feedback.current_state = 'set_action_lib_result' + def _detection_cb(self): + self._detecting_done = True + def set_result(self, success, recognized_planes): result = PerceivePlaneResult() - result.success = self.result - if ud.recognized_planes is None: + result.success = success + if recognized_planes is None: result.recognized_planes = PlaneList() result.recognized_planes.planes = [] else: - result.recognized_planes = ud.recognized_planes - ud.perceive_plane_result = result - return Constant.SUCCESS + result.recognized_planes = recognized_planes + return result diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/CHANGELOG.rst b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/CHANGELOG.rst deleted file mode 100644 index 486eb66f0..000000000 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/CHANGELOG.rst +++ /dev/null @@ -1,34 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package mdr_perceive_table -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.2.0 (2017-11-04) ------------------- -* Move actions into ros folder -* Contributors: Argentina Ortega Sainz - -1.1.2 (2017-10-29) ------------------- -* Add missing scripts to installs in CMakeLists.txt -* Contributors: Argentina Ortega Sainz - -1.1.1 (2017-09-20) ------------------- -* Change maintainer tags to MAS Robotics -* Update changelog for mdr_planning -* Contributors: Argentina Ortega Sainz - -1.1.0 (2017-09-18 18:15) ------------------------- - -1.0.1 (2017-09-18 18:04) ------------------------- -* Added changelogs for mdr_planning and mdr_msgs -* Merge branch 'refactor/mdr_planning' into 'indigo' - Move actions, behaviors, and scenarios to a new mdr_planning metapackage - See merge request !25 -* Refactored mdr_actions (created an mdr_perceive_table package for the perceive table action) -* Contributors: Alex Mitrevski - -1.0.0 (2017-04-11) ------------------- diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/CMakeLists.txt b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/CMakeLists.txt deleted file mode 100644 index 4c441e37c..000000000 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(mdr_perceive_table) - -find_package(catkin REQUIRED COMPONENTS - actionlib - actionlib_msgs - genmsg - message_generation - roslint - rospy -) - -catkin_python_setup() - -add_action_files(DIRECTORY ros/action - FILES - PerceiveTable.action -) - -generate_messages( - DEPENDENCIES - actionlib_msgs -) - -catkin_package( - CATKIN_DEPENDS - actionlib_msgs - message_runtime -) - -include_directories( - ${catkin_INCLUDE_DIRS} -) - -install(PROGRAMS - ros/scripts/perceive_table - ros/scripts/perceive_table_client_test - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/scripts -) diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/package.xml b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/package.xml deleted file mode 100644 index e1da0079f..000000000 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/package.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - mdr_perceive_table - 1.2.0 - An action for perceiving a table - - Santosh Thoduka - Alex Moriarty - Daniel Vazquez - Iryna Ivanovska - Oscar Lima - - MAS robotics - - GPLv3 - - catkin - genmsg - actionlib - actionlib_msgs - message_generation - roslint - rospy - - rospy - actionlib - actionlib_msgs - message_runtime - mcr_perception_msgs - mcr_states - mcr_dynamic_reconfigure_client - - - diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/action/PerceiveTable.action b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/action/PerceiveTable.action deleted file mode 100644 index 48b6b0aff..000000000 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/action/PerceiveTable.action +++ /dev/null @@ -1,9 +0,0 @@ -# goal definition -string location ---- -# result definition -bool success ---- -# feedback -string current_state -string text diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/config/perceive_plane_configurations.yaml b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/config/perceive_plane_configurations.yaml deleted file mode 100644 index b9c0df58f..000000000 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/config/perceive_plane_configurations.yaml +++ /dev/null @@ -1,39 +0,0 @@ -# configuration_name: -# node_name: -# param_name: param_value -# param_name: param_value -# node_name: -# param_name: param_value -# param_name: param_value -# configuration_name: -# node_name: -# param_name: param_value -# param_name: param_value -# node_name: -# param_name: param_value -# param_name: param_value - -shelf_1: - /mcr_perception/voxel_filter: - filter_limit_min: 0.3 - filter_limit_max: 0.6 -shelf_2: - /mcr_perception/voxel_filter: - filter_limit_min: 0.3 - filter_limit_max: 0.6 -shelf_3: - /mcr_perception/voxel_filter: - filter_limit_min: 0.6 - filter_limit_max: 0.9 -shelf_4: - /mcr_perception/voxel_filter: - filter_limit_min: 0.9 - filter_limit_max: 1.3 -shelf_5: - /mcr_perception/voxel_filter: - filter_limit_min: 1.2 - filter_limit_max: 1.8 -table: - /mcr_perception/voxel_filter: - filter_limit_min: 0.6 - filter_limit_max: 1.5 diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/launch/perceive_table.launch b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/launch/perceive_table.launch deleted file mode 100644 index 062254f84..000000000 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/launch/perceive_table.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/scripts/perceive_table b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/scripts/perceive_table deleted file mode 100755 index 671bd7666..000000000 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/scripts/perceive_table +++ /dev/null @@ -1,90 +0,0 @@ -#!/usr/bin/env python -import rospy -import smach - -import mdr_common_states.common_states_manipulation as gms -import mcr_states.common.perception_states as gps -import mcr_states.common.basic_states as gbs - -# action lib -from smach_ros import ActionServerWrapper -from mdr_perceive_table.msg import PerceiveTableAction -from mdr_perceive_table.msg import PerceiveTableFeedback -from mdr_perceive_table.msg import PerceiveTableResult - - -class SetActionLibResult(smach.State): - def __init__(self, result): - smach.State.__init__(self, outcomes=['succeeded'], - input_keys=['perceive_table_goal'], - output_keys=['perceive_table_feedback', 'perceive_table_result']) - self.result = result - - def execute(self, userdata): - result = PerceiveTableResult() - result.success = self.result - userdata.perceive_table_result = result - return 'succeeded' - - -def main(): - rospy.init_node('perceive_table_server') - # Construct state machine - sm = smach.StateMachine( - outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'], - input_keys=['perceive_table_goal'], - output_keys=['perceive_table_feedback', 'perceive_table_result']) - with sm: - smach.StateMachine.add('MOVE_ARM', gms.move_arm('look_at_table_front'), - transitions={'succeeded': 'MOVE_TORSO_BACK', - 'failed': 'MOVE_TORSO_FRONT'}) - - smach.StateMachine.add('MOVE_TORSO_FRONT', gms.move_part('torso', 'front'), - transitions={'succeeded': 'MOVE_ARM', - 'failed': 'MOVE_TORSO_FRONT'}) - - smach.StateMachine.add('MOVE_TORSO_BACK', gms.move_part('torso', 'back_extreme'), - transitions={'succeeded': 'MOVE_HEAD_BACK', - 'failed': 'MOVE_TORSO_BACK'}) - - smach.StateMachine.add('MOVE_HEAD_BACK', gms.move_part('head', 'back_table'), - transitions={'succeeded': 'CONFIGURE_TABLE', - 'failed': 'MOVE_HEAD_BACK'}) - - smach.StateMachine.add('CONFIGURE_TABLE', gbs.set_named_config('table'), - transitions={'success': 'START_WORKSPACE_FINDER', - 'failure': 'SET_ACTION_LIB_FAILURE', - 'timeout': 'CONFIGURE_TABLE'}) - - smach.StateMachine.add('START_WORKSPACE_FINDER', gbs.send_event( - event_list=[('/mcr_perception/mux_pointcloud/select', '/cam3d/depth_registered/points')]), - transitions={'success': 'RECOGNIZE_OBJECTS'}) - - smach.StateMachine.add('RECOGNIZE_OBJECTS', gps.find_objects(retries=1), - transitions={'objects_found': 'SET_ACTION_LIB_SUCCESS', - 'no_objects_found': 'SET_ACTION_LIB_FAILURE'}, - remapping={'found_objects': 'recognized_objects'}) - - smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), - transitions={'succeeded': 'OVERALL_SUCCESS'}) - - smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False), - transitions={'succeeded': 'OVERALL_FAILED'}) - - # Construct action server wrapper - asw = ActionServerWrapper( - server_name='perceive_table_server', - action_spec=PerceiveTableAction, - wrapped_container=sm, - succeeded_outcomes=['OVERALL_SUCCESS'], - aborted_outcomes=['OVERALL_FAILED'], - preempted_outcomes=['PREEMPTED'], - goal_key='perceive_table_goal', - feedback_key='perceive_table_feedback', - result_key='perceive_table_result') - # Run the server in a background thread - asw.run_server() - rospy.spin() - -if __name__ == '__main__': - main() diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/scripts/perceive_table_client_test b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/scripts/perceive_table_client_test deleted file mode 100755 index 100c8475c..000000000 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/scripts/perceive_table_client_test +++ /dev/null @@ -1,23 +0,0 @@ -#! /usr/bin/env python -import rospy -import roslib -import actionlib - -import sys - -from mdr_perceive_table.msg import PerceiveTableAction, PerceiveTableGoal - -if __name__ == '__main__': - rospy.init_node('perceive_table_client_tester') - client = actionlib.SimpleActionClient('/mdr_actions/perceive_table_server', PerceiveTableAction) - client.wait_for_server() - goal = PerceiveTableGoal() - goal.location = 'anywhere' - try: - timeout = 45.0 - rospy.loginfo('Sending action lib goal to perceive_table_server') - client.send_goal(goal) - client.wait_for_result(rospy.Duration.from_sec(int(timeout))) - print client.get_result() - except: - pass diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/src/mdr_perceive_table/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/src/mdr_perceive_table/action_states.py deleted file mode 100644 index c43a228b1..000000000 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/ros/src/mdr_perceive_table/action_states.py +++ /dev/null @@ -1,27 +0,0 @@ -#!/usr/bin/python - -import rospy -import smach -import smach_ros -import actionlib -import mdr_perceive_table.msg - -class perceive_table(smach.State): - def __init__(self, timeout=15.0, action_server='perceive_table_server'): - smach.State.__init__(self, outcomes=['succeeded', 'failed']) - self.timeout = timeout - self.action_server = action_server - self.client = actionlib.SimpleActionClient(action_server, mdr_perceive_table.msg.PerceiveTableAction) - self.client.wait_for_server() - - def execute(self, userdata): - goal = mdr_perceive_table.msg.PerceiveTableGoal() - goal.location = 'anywhere' - rospy.loginfo('Sending actionlib goal to ' + self.action_server + ' with timeout: ' + str(self.timeout)) - self.client.send_goal(goal) - self.client.wait_for_result(rospy.Duration.from_sec(self.timeout)) - res = self.client.get_result() - if res and res.success: - return 'succeeded' - else: - return 'failed' diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_recognize_emotion_action/ros/scripts/recognize_emotion_action b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_recognize_emotion_action/ros/scripts/recognize_emotion_action index f604ad899..c3ec5f57e 100755 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_recognize_emotion_action/ros/scripts/recognize_emotion_action +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_recognize_emotion_action/ros/scripts/recognize_emotion_action @@ -1,73 +1,57 @@ #!/usr/bin/env python import rospy -import smach - -from smach_ros import ActionServerWrapper, IntrospectionServer +import actionlib from mdr_recognize_emotion_action.msg import RecognizeEmotionAction -from mdr_recognize_emotion_action.action_states import (SetupRecognizeEmotion, RecognizeEmotion, - SetActionLibResult) +from mdr_recognize_emotion_action.action_states import RecognizeEmotionSM + +class RecognizeEmotionServer(object): + '''A server exposing an action for recognizing the emotions of previously detected people. + The server expects the following parameters to be made available on the ROS parameter server: + * emotion_model_path: Absolute path of an emotion recognition model (default '') + * image_topic: Name of a camera image topic (default '/cam3d/rgb/image_raw') -class RecognizeEmotionSkill(smach.StateMachine): - def __init__(self, timeout=10): - smach.StateMachine.__init__(self, - outcomes=['OVERALL_SUCCESS', - 'OVERALL_FAILED', 'PREEMPTED'], - input_keys=['recognize_emotion_goal'], - output_keys=['recognize_emotion_feedback', - 'recognize_emotion_result']) + @author Alex Mitrevski, Octavio Arriaga + @contact aleksandar.mitrevski@h-brs.de + ''' + def __init__(self): emotion_model_path = rospy.get_param('~emotion_model_path', '') image_topic = rospy.get_param('~image_topic', '/cam3d/rgb/image_raw') labels = {0: 'angry', 1: 'disgusted', 2: 'sad', 3: 'happy', 4: 'sad', 5: 'surprised', 6: 'neutral'} image_size = (48, 48, 1) - with self: - smach.StateMachine.add('SETUP_RECOGNIZE_EMOTION', - SetupRecognizeEmotion(), - transitions={'succeeded': 'RECOGNIZE_EMOTION', - 'failed': 'SETUP_RECOGNIZE_EMOTION'}) - - smach.StateMachine.add('RECOGNIZE_EMOTION', - RecognizeEmotion(emotion_model_path=emotion_model_path, - image_topic=image_topic, - labels=labels, image_size=image_size), - transitions={'succeeded': 'SET_ACTION_LIB_SUCCESS', - 'failed': 'SET_ACTION_LIB_FAILED'}) - - smach.StateMachine.add('SET_ACTION_LIB_FAILED', - SetActionLibResult(False), - transitions={'succeeded': 'OVERALL_FAILED'}) - - smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', - SetActionLibResult(True), - transitions={'succeeded': 'OVERALL_SUCCESS'}) - + rospy.loginfo('[recognize_emotion] Initialising state machine') + self.action_sm = RecognizeEmotionSM(emotion_model_path=emotion_model_path, + image_topic=image_topic, + labels=labels, + image_size=image_size) + rospy.loginfo('[recognize_emotion] State machine initialised') + + self.action_server = actionlib.SimpleActionServer('recognize_emotion_server', + RecognizeEmotionAction, + self.execute, False) + self.action_server.start() + rospy.loginfo('recognize_emotion action server ready') + + def execute(self, goal): + rospy.loginfo('[recognize_emotion] Received an action request') + self.action_sm.goal = goal + self.action_sm.result = None + self.action_sm.execution_requested = True + while not self.action_sm.result: + rospy.sleep(0.05) + self.action_server.set_succeeded(self.action_sm.result) if __name__ == '__main__': rospy.init_node('recognize_emotion_server') - - # construct state machine - sm = RecognizeEmotionSkill() - - # smach viewer - sis = IntrospectionServer('recognize_emotion_smach_viewer', sm, - '/recognize_emotion_SMACH_VIEWER') - sis.start() - - asw = ActionServerWrapper( - server_name='recognize_emotion_server', - action_spec=RecognizeEmotionAction, - wrapped_container=sm, - succeeded_outcomes=['OVERALL_SUCCESS'], - aborted_outcomes=['OVERALL_FAILED'], - preempted_outcomes=['PREEMPTED'], - goal_key='recognize_emotion_goal', - feedback_key='recognize_emotion_feedback', - result_key='recognize_emotion_result') - - # Run the server in a background thread - asw.run_server() - rospy.spin() + recognize_emotion_server = RecognizeEmotionServer() + try: + recognize_emotion_server.action_sm.run() + while recognize_emotion_server.action_sm.is_running and not rospy.is_shutdown(): + rospy.spin() + except (KeyboardInterrupt, SystemExit): + print('{0} interrupted; exiting...'.format(recognize_emotion_server.action_sm.name)) + recognize_emotion_server.action_sm.stop() diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_recognize_emotion_action/ros/src/mdr_recognize_emotion_action/action_states.py b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_recognize_emotion_action/ros/src/mdr_recognize_emotion_action/action_states.py index 280920551..572a6dd16 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_recognize_emotion_action/ros/src/mdr_recognize_emotion_action/action_states.py +++ b/mdr_planning/mdr_actions/mdr_perception_actions/mdr_recognize_emotion_action/ros/src/mdr_recognize_emotion_action/action_states.py @@ -1,9 +1,5 @@ #!/usr/bin/python - import rospy -import smach -import smach_ros -import actionlib from sensor_msgs.msg import Image import numpy as np @@ -12,67 +8,57 @@ from keras.models import load_model from cv_bridge import CvBridge, CvBridgeError -from mdr_recognize_emotion_action.msg import (RecognizeEmotionFeedback, - RecognizeEmotionResult) - - -class SetupRecognizeEmotion(smach.State): - def __init__(self): - smach.State.__init__(self, outcomes=['succeeded', 'failed'], - input_keys=['recognize_emotion_goal'], - output_keys=['recognize_emotion_feedback', - 'recognize_emotion_result']) - - def execute(self, userdata): - feedback = RecognizeEmotionFeedback() - feedback.current_state = 'RECOGNIZE_EMOTION' - feedback.message = '[recognize_emotion] Recongnizing emotion' - userdata.recognize_emotion_feedback = feedback - return 'succeeded' - - -class RecognizeEmotion(smach.State): - def __init__(self, timeout=120.0, image_topic='/cam3d/rgb/image_raw', - emotion_model_path=None, labels=dict(), image_size=(0, 0, 0)): - smach.State.__init__(self, input_keys=['recognize_emotion_goal', - 'emotions'], - output_keys=['recognize_emotion_feedback', - 'emotions'], - outcomes=['succeeded', 'failed']) +from pyftsm.ftsm import FTSMTransitions +from mas_execution.action_sm_base import ActionSMBase +from mdr_recognize_emotion_action.msg import RecognizeEmotionFeedback, RecognizeEmotionResult + +class RecognizeEmotionSM(ActionSMBase): + def __init__(self, timeout=120.0, + image_topic='/cam3d/rgb/image_raw', + emotion_model_path=None, + labels=None, + image_size=(0, 0, 0), + max_recovery_attempts=1): + super(RecognizeEmotionSM, self).__init__('RecognizeEmotion', [], max_recovery_attempts) self.timeout = timeout self.labels = labels self.image_size = image_size self.image_publisher = rospy.Publisher(image_topic, Image, queue_size=1) + self.emotion_model_path = emotion_model_path self.bridge = CvBridge() + self.emotion_model = None + self.computation_graph = None + def init(self): try: + rospy.loginfo('[recognize_emotion] Loading model %s', self.emotion_model_path) self.emotion_model = load_model(emotion_model_path) - print('Model ' + emotion_model_path + ' loaded successfully') - # the following two lines (and the with... line in predict_emotion) - # are necessary for avoiding + # the following two lines are necessary for avoiding # https://github.com/keras-team/keras/issues/2397 self.emotion_model._make_predict_function() self.computation_graph = tf.get_default_graph() except: - print('Failed to load model ', emotion_model_path) + rospy.logerr('[recognize_emotion] Model %s could not be loaded', self.emotion_model_path) + return FTSMTransitions.INITIALISED - def execute(self, userdata): - number_of_faces = userdata.recognize_emotion_goal.number_of_faces - bounding_boxes = userdata.recognize_emotion_goal.bounding_boxes - userdata.emotions = list() + def running(self): + number_of_faces = self.goal.number_of_faces + bounding_boxes = self.goal.bounding_boxes + emotions = [] - rgb_image = self.ros2cv(userdata.recognize_emotion_goal.image) - gray_image = self.rgb2gray(rgb_image) + rospy.loginfo('[recognize_emotion] Recognizing emotions') + rgb_image = self.__ros2cv(self.goal.image) + gray_image = self.__rgb2gray(rgb_image) for face in bounding_boxes: x, y, w, h = face.bounding_box_coordinates face = gray_image[y: (y + h), x: (x + w)] face = cv2.resize(face, self.image_size[0:2]) face = np.expand_dims(face, 0) face = np.expand_dims(face, -1) - face = self.preprocess_image(face) - predicted_emotion = self.predict_emotion(face) - userdata.emotions.append(predicted_emotion) + face = self.__preprocess_image(face) + predicted_emotion = self.__predict_emotion(face) + emotions.append(predicted_emotion) rgb_cv2 = cv2.rectangle(rgb_image, (x, y), (x + w, y + h), (0, 0, 255), 2) @@ -81,39 +67,31 @@ def execute(self, userdata): 1, cv2.LINE_AA) output_ros_image = self.bridge.cv2_to_imgmsg(rgb_image, 'bgr8') self.image_publisher.publish(output_ros_image) - return 'succeeded' - def preprocess_image(self, image): + self.result = self.set_result(True, emotions) + return FTSMTransitions.DONE + + def set_result(self, success, emotions): + result = RecognizeEmotionResult() + result.success = success + result.emotions = emotions + return result + + def __preprocess_image(self, image): image = image / 255.0 return image - def predict_emotion(self, face): + def __predict_emotion(self, face): label = -1 with self.computation_graph.as_default(): class_predictions = self.emotion_model.predict(face) label = self.labels[np.argmax(class_predictions)] return label - def ros2cv(self, ros_image): + def __ros2cv(self, ros_image): cv_image = self.bridge.imgmsg_to_cv2(ros_image, 'bgr8') return np.array(cv_image, dtype=np.uint8) - def rgb2gray(self, image): + def __rgb2gray(self, image): gray_image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) return gray_image - - -class SetActionLibResult(smach.State): - def __init__(self, result): - smach.State.__init__(self, outcomes=['succeeded'], - input_keys=['recognize_emotion_goal', 'emotions'], - output_keys=['recognize_emotion_feedback', - 'recognize_emotion_result']) - self.result = result - - def execute(self, userdata): - result = RecognizeEmotionResult() - result.success = self.result - result.emotions = userdata.emotions - userdata.recognize_emotion_result = result - return 'succeeded' diff --git a/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/CMakeLists.txt b/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/CMakeLists.txt index 4bfac3c1c..2fa303bb0 100644 --- a/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/CMakeLists.txt +++ b/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/CMakeLists.txt @@ -8,6 +8,8 @@ find_package(catkin REQUIRED COMPONENTS message_generation roslint rospy + mdr_speech_matching + mdr_speech_recognition ) catkin_python_setup() @@ -28,6 +30,8 @@ catkin_package( actionlib_msgs message_runtime std_msgs + mdr_speech_matching + mdr_speech_recognition ) include_directories( diff --git a/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/package.xml b/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/package.xml index e7f31d20b..19ce45cd9 100644 --- a/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/package.xml +++ b/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/package.xml @@ -15,12 +15,16 @@ message_generation roslint rospy + mdr_speech_matching + mdr_speech_recognition rospy actionlib actionlib_msgs message_runtime std_msgs + mdr_speech_matching + mdr_speech_recognition diff --git a/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/ros/scripts/listen_client_test b/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/ros/scripts/listen_client_test index 5c4e97f49..e7088729e 100755 --- a/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/ros/scripts/listen_client_test +++ b/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/ros/scripts/listen_client_test @@ -38,7 +38,7 @@ if __name__ == '__main__': timeout = 15.0 client.wait_for_result(rospy.Duration.from_sec(int(timeout))) - rospy.loginfo(client.get_result()) + rospy.loginfo(client.get_result()) except Exception as ex: rospy.logerr(type(ex).__name__ + ": You have failed!") diff --git a/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/setup.py b/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/setup.py index 3b48c27ce..e938e3048 100644 --- a/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/setup.py +++ b/mdr_planning/mdr_actions/mdr_speech_actions/mdr_listen_action/setup.py @@ -4,8 +4,7 @@ from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( - packages=['mdr_listen_action', 'mdr_listen_action_ros'], - package_dir={'mdr_listen_action': 'common/src/mdr_listen_action', - 'mdr_listen_action_ros': 'ros/src/mdr_listen_action_ros'}) + packages=['mdr_listen_action_ros'], + package_dir={'': 'ros/src/'}) setup(**d) diff --git a/mdr_planning/mdr_behaviours/README.md b/mdr_planning/mdr_behaviours/README.md new file mode 100644 index 000000000..1b73c3379 --- /dev/null +++ b/mdr_planning/mdr_behaviours/README.md @@ -0,0 +1,65 @@ +# `mdr_behaviours` + +## Table of contents + +1. [Summary](#summary) +2. [Organisation](#organisation) +3. [Dependencies](#dependencies) +4. [Implemented behaviours](#implemented-behaviours) + 1. [mdr_navigation_behaviours](#mdr_navigation_behaviours) + 2. [mdr_perception_behaviours](#mdr_perception_behaviours) + 3. [mdr_manipulation_behaviours](#mdr_manipulation_behaviours) + 4. [mdr_knowledge_behaviours](#mdr_knowledge_behaviours) + +## Summary + +Most domestic scenarios include common components, such as going to certain locations or manipulating objects in a certain way, The `mdr_behaviours` metapackage groups together such functionalities, thereby facilitating the reusability of components when building complex robot scenarios and minimising unnecessary code repetition in the process. + +The name `behaviours` indicates that the components included in this package make robots behave in a certain manner. In this context, we can define an *atomic behaviour* as a specific way of accomplishing something in a certain way. Under this definition, "picking the closest object from a surface" is a specific robot behaviour that differs from "picking a random object from the surface". Atomic behaviours on their own may not make much sense, but when used in conjunction with other atomic behaviours, they give rise to *complex behaviours* that allow robots to perform complex tasks. For instance, combining the atomic behaviours +1. going to the table +2. turning towards the table +3. locating objects on the table +4. picking the closest object from the table +5. going to a cupboard +6. placing the object in the cupboard + +allows us to define a simple pick-and-place behaviour (as done [here](../mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/config/pick_and_place_sm.yaml)). + +As defined here, the term behaviour has a small semantic difference with our use of the term action as defined in [`mdr_actions`](../mdr_actions), which is why actions and behaviours are kept separately. We use the term action in the classical planning sense, namely an action can be thought of as a function with a certain set of preconditions that, if executed successfully, leads to a given set of effects; on the other hand, a behaviour precedes an action and, roughly speaking, controls what the action does. In the pick-and-place example above, the behaviour of "picking the closest object from the table" precedes a grasping action and defines that a robot should try to grasp the closest rather than a randomly selected object on the table, thereby allowing the robot to make more "optimal" decisions. + +All behaviours included here implement the [`scenario state base class`](https://github.com/b-it-bots/mas_execution_manager/blob/master/ros/src/mas_execution_manager/scenario_state_base.py) in `mas_execution_manager` and interact with the action clients in `mdr_actions`. Each behaviour is implemented in a dedicated Python script. + +## Organisation + +The metapackage is split into the following packages: +* [`mdr_navigation_behaviours`](mdr_navigation_behaviours): A collection of navigation-related behaviours +* [`mdr_perception_behaviours`](mdr_perception_behaviours): Behaviours for perceiving the world in a certain way +* [`mdr_manipulation_behaviours`](mdr_manipulation_behaviours): A group of behaviours for manipulating objects in the world +* [`mdr_knowledge_behaviours`](mdr_knowledge_behaviours): Functionalities that let a robot use its knowledge about the world in order to make informed decisions + +## Dependencies + +* `mas_execution_manager` +* `mas_knowledge_base` +* The actions and action clients in the `mdr_actions` metapackage + +## Implemented behaviours + +### `mdr_navigation_behaviours` + +* `move_base`: Moves through a set of predefined navigation waypoints, updating the location of the robot in the knowledge base during the process + +### `mdr_perception_behaviours` + +* `perceive_planes`: Scans a plane (or a set of planes) assuming that the robot's camera is already oriented towards the plane(s) + +### `mdr_manipulation_behaviours` + +* `pick_closest_from_surface`: Given a surface and a set of objects on it, finds the closest object with respect to the base_link frame of the robot +* `place`: Places an object on a surface whose name starts with a given prefix +* `place_based_on_category`: Places an object on a surface that contains objects of the same category (if objects of the same category are present on multiple surfaces, chooses the surfaces with the highest number of objects of that category) +* `throw_object_in`: Throws an object in a given throwing target + +### `mdr_knowledge_behaviours` + +* `check_empty_surface`: Checks whether a given surface is empty based on the most recent knowledge in the knowledge base diff --git a/mdr_planning/mdr_behaviours/mdr_behaviours/CMakeLists.txt b/mdr_planning/mdr_behaviours/mdr_behaviours/CMakeLists.txt new file mode 100644 index 000000000..5db79dfe2 --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_behaviours/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mdr_behaviours) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/mdr_planning/mdr_behaviours/mdr_behaviours/package.xml b/mdr_planning/mdr_behaviours/mdr_behaviours/package.xml new file mode 100644 index 000000000..910c1c706 --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_behaviours/package.xml @@ -0,0 +1,17 @@ + + + mdr_behaviours + 1.2.0 + Metapackage for packages defining common domestic robot behaviours. + + GPLv3 + + Alex Mitrevski + MAS robotics + + catkin + + + + + diff --git a/mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/CMakeLists.txt b/mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/CMakeLists.txt new file mode 100644 index 000000000..64149274e --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mdr_knowledge_behaviours) + +find_package(catkin REQUIRED COMPONENTS + rospy + roslint + smach + mas_execution_manager +) + +roslint_python() +catkin_python_setup() + +catkin_package( + CATKIN_DEPENDS + rospy + smach + mas_execution_manager +) + +include_directories( + ${catkin_INCLUDE_DIRS} +) diff --git a/mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/package.xml b/mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/package.xml new file mode 100644 index 000000000..f7b45e7bb --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/package.xml @@ -0,0 +1,23 @@ + + + mdr_knowledge_behaviours + 1.2.0 + A package for common knowledge retrieval robot behaviours. + + Alex Mitrevski + MAS robotics + + GPLv3 + + catkin + rospy + roslint + smach + mas_execution_manager + + rospy + smach + mas_execution_manager + + + diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/ros/src/mdr_demo_patrol/scenario_states/__init__.py b/mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/ros/src/mdr_knowledge_behaviours/__init__.py similarity index 100% rename from mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/ros/src/mdr_demo_patrol/scenario_states/__init__.py rename to mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/ros/src/mdr_knowledge_behaviours/__init__.py diff --git a/mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/ros/src/mdr_knowledge_behaviours/check_empty_surface.py b/mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/ros/src/mdr_knowledge_behaviours/check_empty_surface.py new file mode 100644 index 000000000..49f6e46d7 --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/ros/src/mdr_knowledge_behaviours/check_empty_surface.py @@ -0,0 +1,16 @@ +from mas_execution_manager.scenario_state_base import ScenarioStateBase + +class CheckEmptySurface(ScenarioStateBase): + def __init__(self, save_sm_state=False, **kwargs): + ScenarioStateBase.__init__(self, 'check_empty_surface', + save_sm_state=save_sm_state, + outcomes=['empty', 'not_empty']) + self.sm_id = kwargs.get('sm_id', '') + self.state_name = kwargs.get('state_name', 'check_empty_surface') + self.surface_prefix = kwargs.get('surface_prefix', '') + + def execute(self, userdata): + if self.kb_interface.is_surface_empty(self.surface_prefix): + self.say('{0} is empty'.format(self.surface_prefix)) + return 'empty' + return 'not_empty' diff --git a/mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/setup.py b/mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/setup.py new file mode 100644 index 000000000..0b04f3a10 --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_knowledge_behaviours/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['mdr_knowledge_behaviours'], + package_dir={'mdr_knowledge_behaviours': 'ros/src/mdr_knowledge_behaviours'} +) + +setup(**d) diff --git a/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/CMakeLists.txt b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/CMakeLists.txt new file mode 100644 index 000000000..5be429abb --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mdr_manipulation_behaviours) + +find_package(catkin REQUIRED COMPONENTS + rospy + roslint + smach + mas_execution_manager + mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action + mdr_pickup_action + mdr_place_action +) + +roslint_python() +catkin_python_setup() + +catkin_package( + CATKIN_DEPENDS + rospy + smach + mas_execution_manager + mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action + mdr_pickup_action + mdr_place_action +) + +include_directories( + ${catkin_INCLUDE_DIRS} +) diff --git a/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/package.xml b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/package.xml new file mode 100644 index 000000000..920b47716 --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/package.xml @@ -0,0 +1,33 @@ + + + mdr_manipulation_behaviours + 1.2.0 + A package for common manipulation-related robot behaviours. + + Alex Mitrevski + MAS robotics + + GPLv3 + + catkin + rospy + roslint + smach + mas_execution_manager + mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action + mdr_pickup_action + mdr_place_action + + rospy + smach + mas_execution_manager + mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action + mdr_pickup_action + mdr_place_action + + + diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/__init__.py b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/__init__.py similarity index 100% rename from mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/__init__.py rename to mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/__init__.py diff --git a/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/pick_closest_from_surface.py b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/pick_closest_from_surface.py new file mode 100644 index 000000000..fd1117479 --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/pick_closest_from_surface.py @@ -0,0 +1,117 @@ +import time +import numpy as np +import rospy +from tf import TransformListener + +import rosplan_dispatch_msgs.msg as plan_dispatch_msgs +import diagnostic_msgs.msg as diag_msgs + +from mas_perception_msgs.msg import Object +from mas_execution_manager.scenario_state_base import ScenarioStateBase + +class PickClosestFromSurface(ScenarioStateBase): + def __init__(self, save_sm_state=False, **kwargs): + ScenarioStateBase.__init__(self, 'pickup', + save_sm_state=save_sm_state, + output_keys=['grasped_object'], + outcomes=['succeeded', 'failed', + 'failed_after_retrying', + 'find_objects_before_picking']) + self.sm_id = kwargs.get('sm_id', '') + self.state_name = kwargs.get('state_name', 'pick') + self.timeout = kwargs.get('timeout', 120.) + self.picking_surface_prefix = kwargs.get('picking_surface_prefix', 120.) + self.tf_listener = TransformListener() + + self.number_of_retries = kwargs.get('number_of_retries', 0) + self.retry_count = 0 + + def execute(self, userdata): + if self.save_sm_state: + self.save_current_state() + + surface_objects = self.kb_interface.get_surface_object_map(self.picking_surface_prefix) + object_poses = self.kb_interface.get_surface_object_pose_map(surface_objects, Object._type) + surface, obj_to_grasp = self.select_object_for_grasping(object_poses) + if not obj_to_grasp: + rospy.logerr('Could not find an object to grasp') + self.say('Could not find an object to grasp') + if self.retry_count == self.number_of_retries: + rospy.logerr('No object could be found; giving up') + self.say('Could not find an object to grasp; giving up') + return 'failed_after_retrying' + self.retry_count += 1 + return 'find_objects_before_picking' + + self.retry_count = 0 + dispatch_msg = self.get_dispatch_msg(obj_to_grasp, surface) + rospy.loginfo('Picking %s from %s' % (obj_to_grasp, surface)) + self.say('Picking ' + obj_to_grasp + ' from ' + surface) + self.action_dispatch_pub.publish(dispatch_msg) + + self.executing = True + self.succeeded = False + start_time = time.time() + duration = 0. + while self.executing and duration < self.timeout: + rospy.sleep(0.1) + duration = time.time() - start_time + + if self.succeeded: + rospy.loginfo('%s grasped successfully' % obj_to_grasp) + self.say('Successfully grasped ' + obj_to_grasp) + userdata.grasped_object = obj_to_grasp + return 'succeeded' + + rospy.loginfo('Could not grasp %s' % obj_to_grasp) + self.say('Could not grasp ' + obj_to_grasp) + if self.retry_count == self.number_of_retries: + rospy.loginfo('Failed to grasp %s' % obj_to_grasp) + self.say('Aborting operation') + return 'failed_after_retrying' + rospy.loginfo('Retrying to grasp %s' % obj_to_grasp) + self.retry_count += 1 + return 'failed' + + def select_object_for_grasping(self, surface_object_poses): + '''Returns the index of the object whose distance is closest to the robot + ''' + robot_position = np.zeros(3) + min_dist = 1e100 + min_dist_obj = '' + obj_surface = '' + for surface, object_poses in surface_object_poses.items(): + for obj, pose in object_poses.items(): + base_link_pose = self.tf_listener.transformPose('base_link', pose) + dist = self.distance(robot_position, np.array([base_link_pose.pose.position.x, + base_link_pose.pose.position.y, + base_link_pose.pose.position.z])) + if dist < min_dist: + min_dist = dist + min_dist_obj = obj + obj_surface = surface + return obj_surface, min_dist_obj + + def distance(self, point1, point2): + return np.linalg.norm(np.array(point1) - np.array(point2)) + + def get_dispatch_msg(self, obj_name, surface_name): + dispatch_msg = plan_dispatch_msgs.ActionDispatch() + dispatch_msg.name = self.action_name + + arg_msg = diag_msgs.KeyValue() + arg_msg.key = 'bot' + arg_msg.value = self.robot_name + dispatch_msg.parameters.append(arg_msg) + + arg_msg = diag_msgs.KeyValue() + arg_msg.key = 'obj' + arg_msg.value = obj_name + dispatch_msg.parameters.append(arg_msg) + + arg_msg = diag_msgs.KeyValue() + arg_msg.key = 'plane' + arg_msg.value = surface_name + dispatch_msg.parameters.append(arg_msg) + + return dispatch_msg diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/place.py b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/place.py similarity index 74% rename from mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/place.py rename to mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/place.py index d0c7d4e69..c57f965d8 100644 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/place.py +++ b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/place.py @@ -1,10 +1,7 @@ import time -import numpy as np import rospy -from std_msgs.msg import String import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import rosplan_knowledge_msgs.srv as rosplan_srvs import diagnostic_msgs.msg as diag_msgs from mas_execution_manager.scenario_state_base import ScenarioStateBase @@ -14,10 +11,11 @@ def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__(self, 'place', save_sm_state=save_sm_state, input_keys=['grasped_object'], - outcomes=['pick_new_object', 'failed', + outcomes=['succeeded', 'failed', 'failed_after_retrying']) - self.sm_id = kwargs.get('sm_id', 'mdr_demo_simple_pick_and_place') + self.sm_id = kwargs.get('sm_id', '') self.state_name = kwargs.get('state_name', 'place') + self.placing_surface_prefix = kwargs.get('placing_surface_prefix', '') self.timeout = kwargs.get('timeout', 120.) self.number_of_retries = kwargs.get('number_of_retries', 0) @@ -28,7 +26,7 @@ def execute(self, userdata): self.save_current_state() grasped_object = userdata.grasped_object - surface_name = self.get_placing_surface(grasped_object, surface_prefix='table') + surface_name = self.kb_interface.get_surface_name(self.placing_surface_prefix) dispatch_msg = self.get_dispatch_msg(grasped_object, surface_name) rospy.loginfo('Placing %s on %s' % (grasped_object, surface_name)) self.say('Placing ' + grasped_object + ' on ' + surface_name) @@ -45,10 +43,10 @@ def execute(self, userdata): if self.succeeded: rospy.loginfo('Object placed successfully') self.say('Successfully placed ' + grasped_object) - return 'pick_new_object' + return 'succeeded' rospy.loginfo('Could not place object %s' % grasped_object) - self.say('Could not place ' + obj_to_grasp) + self.say('Could not place ' + grasped_object) if self.retry_count == self.number_of_retries: rospy.loginfo('Failed to place object %s' % grasped_object) self.say('Aborting operation') @@ -57,16 +55,6 @@ def execute(self, userdata): self.retry_count += 1 return 'failed' - def get_placing_surface(self, obj_name, surface_prefix='table'): - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = 'explored' - explored_result = self.attribute_fetching_client(request) - for item in explored_result.attributes: - surface_name = '' - for param in item.values: - if param.key == 'plane' and param.value.find(surface_prefix) != -1: - return param.value - def get_dispatch_msg(self, obj_name, plane_name): dispatch_msg = plan_dispatch_msgs.ActionDispatch() dispatch_msg.name = self.action_name diff --git a/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/place_based_on_category.py b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/place_based_on_category.py new file mode 100644 index 000000000..29c551c77 --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/place_based_on_category.py @@ -0,0 +1,109 @@ +import time +import numpy as np +import rospy + +import rosplan_dispatch_msgs.msg as plan_dispatch_msgs +import diagnostic_msgs.msg as diag_msgs + +from mas_execution_manager.scenario_state_base import ScenarioStateBase + +class PlaceBasedOnCategory(ScenarioStateBase): + def __init__(self, save_sm_state=False, **kwargs): + ScenarioStateBase.__init__(self, 'place', + save_sm_state=save_sm_state, + input_keys=['grasped_object'], + outcomes=['succeeded', 'failed', + 'failed_after_retrying']) + self.sm_id = kwargs.get('sm_id', '') + self.state_name = kwargs.get('state_name', 'place_based_on_category') + self.placing_surface_prefix = kwargs.get('placing_surface_prefix', '') + self.timeout = kwargs.get('timeout', 120.) + + self.number_of_retries = kwargs.get('number_of_retries', 0) + self.retry_count = 0 + + def execute(self, userdata): + if self.save_sm_state: + self.save_current_state() + + grasped_object = userdata.grasped_object + surface_name = self.choose_placing_surface(grasped_object) + dispatch_msg = self.get_dispatch_msg(grasped_object, surface_name) + rospy.loginfo('Placing %s on %s' % (grasped_object, surface_name)) + self.say('Placing ' + grasped_object + ' on ' + surface_name) + self.action_dispatch_pub.publish(dispatch_msg) + + self.executing = True + self.succeeded = False + start_time = time.time() + duration = 0. + while self.executing and duration < self.timeout: + rospy.sleep(0.1) + duration = time.time() - start_time + + if self.succeeded: + rospy.loginfo('Object placed successfully') + self.say('Successfully placed ' + grasped_object) + return 'succeeded' + + rospy.loginfo('Could not place object %s' % grasped_object) + self.say('Could not place ' + grasped_object) + if self.retry_count == self.number_of_retries: + rospy.loginfo('Failed to place object %s' % grasped_object) + self.say('Aborting operation') + return 'failed_after_retrying' + rospy.loginfo('Retrying to place object %s' % grasped_object) + self.retry_count += 1 + return 'failed' + + def choose_placing_surface(self, obj_name): + obj_category_map = self.kb_interface.get_obj_category_map() + surface_category_counts = self.kb_interface.get_surface_category_counts(self.placing_surface_prefix, + obj_category_map) + grasped_object_category = obj_category_map[obj_name] + placing_surface = self.get_best_placing_surface(grasped_object_category, + surface_category_counts) + return placing_surface + + def get_best_placing_surface(self, obj_category, surface_category_counts): + '''If none of the surfaces contain objects whose category is + 'obj_category', returns the name of a random surface; otherwise, + returns the name of the surface that has the largest number + of objects of the category 'obj_category'. + ''' + surfaces = list() + obj_surface_category_counts = list() + for surface, category_counts in surface_category_counts.items(): + surfaces.append(surface) + if obj_category in category_counts: + obj_surface_category_counts.append(category_counts[obj_category]) + else: + obj_surface_category_counts.append(0) + + surface_idx = -1 + if np.count_nonzero(obj_surface_category_counts) != 0: + surface_idx = np.argmax(obj_surface_category_counts) + else: + surface_idx = np.random.randint(0, len(surfaces)) + return surfaces[surface_idx] + + def get_dispatch_msg(self, obj_name, plane_name): + dispatch_msg = plan_dispatch_msgs.ActionDispatch() + dispatch_msg.name = self.action_name + + arg_msg = diag_msgs.KeyValue() + arg_msg.key = 'bot' + arg_msg.value = self.robot_name + dispatch_msg.parameters.append(arg_msg) + + arg_msg = diag_msgs.KeyValue() + arg_msg.key = 'obj' + arg_msg.value = obj_name + dispatch_msg.parameters.append(arg_msg) + + arg_msg = diag_msgs.KeyValue() + arg_msg.key = 'plane' + arg_msg.value = plane_name + dispatch_msg.parameters.append(arg_msg) + + return dispatch_msg diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/throw.py b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/throw_object_in.py similarity index 86% rename from mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/throw.py rename to mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/throw_object_in.py index 93a78edae..1508810fe 100644 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/throw.py +++ b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/ros/src/mdr_manipulation_behaviours/throw_object_in.py @@ -1,24 +1,20 @@ import time -import numpy as np import rospy -from std_msgs.msg import String import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import rosplan_knowledge_msgs.srv as rosplan_srvs import diagnostic_msgs.msg as diag_msgs -from mcr_perception_msgs.msg import Object from mas_execution_manager.scenario_state_base import ScenarioStateBase -class Throw(ScenarioStateBase): +class ThrowObjectIn(ScenarioStateBase): def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__(self, 'throw', save_sm_state=save_sm_state, input_keys=['grasped_object'], outcomes=['succeeded', 'failed', 'failed_after_retrying']) - self.sm_id = kwargs.get('sm_id', 'mdr_demo_throw_table_objects') + self.sm_id = kwargs.get('sm_id', '') self.state_name = kwargs.get('state_name', 'throw') - self.throwing_target_name = kwargs.get('throwing_target_name', 'trash_can') + self.throwing_target_name = kwargs.get('throwing_target_name', '') self.timeout = kwargs.get('timeout', 120.) self.number_of_retries = kwargs.get('number_of_retries', 0) @@ -30,8 +26,8 @@ def execute(self, userdata): grasped_object = userdata.grasped_object dispatch_msg = self.get_dispatch_msg(grasped_object) - rospy.loginfo('Throwing %s in the trash can' % grasped_object) - self.say('Throwing ' + grasped_object + ' in the trash can') + rospy.loginfo('Throwing %s in %s' % (grasped_object, self.throwing_target_name)) + self.say('Throwing {0} in {1}'.format(grasped_object, self.throwing_target_name)) self.action_dispatch_pub.publish(dispatch_msg) self.executing = True diff --git a/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/setup.py b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/setup.py new file mode 100644 index 000000000..0fdc3c40d --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_manipulation_behaviours/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['mdr_manipulation_behaviours'], + package_dir={'mdr_manipulation_behaviours': 'ros/src/mdr_manipulation_behaviours'} +) + +setup(**d) diff --git a/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/CMakeLists.txt b/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/CMakeLists.txt new file mode 100644 index 000000000..ed5b4ddf3 --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mdr_navigation_behaviours) + +find_package(catkin REQUIRED COMPONENTS + rospy + roslint + smach + mas_execution_manager + mdr_move_arm_action + mdr_move_base_action +) + +roslint_python() +catkin_python_setup() + +catkin_package( + CATKIN_DEPENDS + rospy + smach + mas_execution_manager + mdr_move_arm_action + mdr_move_base_action +) + +include_directories( + ${catkin_INCLUDE_DIRS} +) diff --git a/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/package.xml b/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/package.xml new file mode 100644 index 000000000..913b5ba2e --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/package.xml @@ -0,0 +1,27 @@ + + + mdr_navigation_behaviours + 1.2.0 + A package for common navigation-related robot behaviours. + + Alex Mitrevski + MAS robotics + + GPLv3 + + catkin + rospy + roslint + smach + mas_execution_manager + mdr_move_arm_action + mdr_move_base_action + + rospy + smach + mas_execution_manager + mdr_move_arm_action + mdr_move_base_action + + + diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/__init__.py b/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/ros/src/mdr_navigation_behaviours/__init__.py similarity index 100% rename from mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/__init__.py rename to mdr_planning/mdr_behaviours/mdr_navigation_behaviours/ros/src/mdr_navigation_behaviours/__init__.py diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/ros/src/mdr_demo_patrol/scenario_states/move_base.py b/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/ros/src/mdr_navigation_behaviours/move_base.py similarity index 79% rename from mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/ros/src/mdr_demo_patrol/scenario_states/move_base.py rename to mdr_planning/mdr_behaviours/mdr_navigation_behaviours/ros/src/mdr_navigation_behaviours/move_base.py index cb53b95eb..d248a1f27 100644 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/ros/src/mdr_demo_patrol/scenario_states/move_base.py +++ b/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/ros/src/mdr_navigation_behaviours/move_base.py @@ -3,7 +3,6 @@ import actionlib import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import rosplan_knowledge_msgs.srv as rosplan_srvs import diagnostic_msgs.msg as diag_msgs from mdr_move_base_action.msg import MoveBaseAction @@ -14,7 +13,7 @@ def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__(self, 'move_base', save_sm_state=save_sm_state, outcomes=['succeeded', 'failed', 'failed_after_retrying']) - self.sm_id = kwargs.get('sm_id', 'mdr_demo_patrol') + self.sm_id = kwargs.get('sm_id', '') self.state_name = kwargs.get('state_name', 'move_base') self.move_base_server = kwargs.get('move_base_server', 'move_base_server') self.destination_locations = list(kwargs.get('destination_locations', list())) @@ -28,23 +27,7 @@ def __init__(self, save_sm_state=False, **kwargs): self.move_base_client.wait_for_server() def execute(self, userdata): - # if the current location of the robot was not specified - # in the request, we query this information from the knowledge base - original_location = '' - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = 'robot_at' - result = self.attribute_fetching_client(request) - for item in result.attributes: - if not item.is_negative: - for param in item.values: - if param.key == 'bot' and param.value != self.robot_name: - break - - if param.key == 'wp': - original_location = param.value - break - break - + original_location = self.kb_interface.get_robot_location(self.robot_name) for destination_location in self.destination_locations: dispatch_msg = self.get_dispatch_msg(original_location, destination_location) diff --git a/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/setup.py b/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/setup.py new file mode 100644 index 000000000..a6a28bc37 --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_navigation_behaviours/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['mdr_navigation_behaviours'], + package_dir={'mdr_navigation_behaviours': 'ros/src/mdr_navigation_behaviours'} +) + +setup(**d) diff --git a/mdr_planning/mdr_behaviours/mdr_perception_behaviours/CMakeLists.txt b/mdr_planning/mdr_behaviours/mdr_perception_behaviours/CMakeLists.txt new file mode 100644 index 000000000..283fd5b9e --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_perception_behaviours/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mdr_perception_behaviours) + +find_package(catkin REQUIRED COMPONENTS + rospy + roslint + smach + mas_execution_manager + mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action +) + +roslint_python() +catkin_python_setup() + +catkin_package( + CATKIN_DEPENDS + rospy + smach + mas_execution_manager + mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action +) + +include_directories( + ${catkin_INCLUDE_DIRS} +) diff --git a/mdr_planning/mdr_behaviours/mdr_perception_behaviours/package.xml b/mdr_planning/mdr_behaviours/mdr_perception_behaviours/package.xml new file mode 100644 index 000000000..9619b511d --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_perception_behaviours/package.xml @@ -0,0 +1,29 @@ + + + mdr_perception_behaviours + 1.2.0 + A package for common perception-related robot behaviours. + + Alex Mitrevski + MAS robotics + + GPLv3 + + catkin + rospy + roslint + smach + mas_execution_manager + mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action + + rospy + smach + mas_execution_manager + mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action + + + diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/__init__.py b/mdr_planning/mdr_behaviours/mdr_perception_behaviours/ros/src/mdr_perception_behaviours/__init__.py similarity index 100% rename from mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/__init__.py rename to mdr_planning/mdr_behaviours/mdr_perception_behaviours/ros/src/mdr_perception_behaviours/__init__.py diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/ros/src/mdr_store_groceries/scenario_states/perceive_planes.py b/mdr_planning/mdr_behaviours/mdr_perception_behaviours/ros/src/mdr_perception_behaviours/perceive_planes.py similarity index 96% rename from mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/ros/src/mdr_store_groceries/scenario_states/perceive_planes.py rename to mdr_planning/mdr_behaviours/mdr_perception_behaviours/ros/src/mdr_perception_behaviours/perceive_planes.py index 0c0ad0990..bf4cd1ad3 100644 --- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/ros/src/mdr_store_groceries/scenario_states/perceive_planes.py +++ b/mdr_planning/mdr_behaviours/mdr_perception_behaviours/ros/src/mdr_perception_behaviours/perceive_planes.py @@ -1,6 +1,5 @@ import time import rospy -from std_msgs.msg import String import rosplan_dispatch_msgs.msg as plan_dispatch_msgs import diagnostic_msgs.msg as diag_msgs @@ -12,7 +11,7 @@ def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__(self, 'perceive_plane', save_sm_state=save_sm_state, outcomes=['succeeded', 'failed', 'failed_after_retrying']) - self.sm_id = kwargs.get('sm_id', 'mdr_store_groceries') + self.sm_id = kwargs.get('sm_id', '') self.state_name = kwargs.get('state_name', 'perceive_planes') self.timeout = kwargs.get('timeout', 120.) self.plane_prefix = kwargs.get('plane_prefix', 0) diff --git a/mdr_planning/mdr_behaviours/mdr_perception_behaviours/setup.py b/mdr_planning/mdr_behaviours/mdr_perception_behaviours/setup.py new file mode 100644 index 000000000..3ab2b293c --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_perception_behaviours/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['mdr_perception_behaviours'], + package_dir={'mdr_perception_behaviours': 'ros/src/mdr_perception_behaviours'} +) + +setup(**d) diff --git a/mdr_planning/mdr_behaviours/mdr_planning_behaviours/CMakeLists.txt b/mdr_planning/mdr_behaviours/mdr_planning_behaviours/CMakeLists.txt new file mode 100644 index 000000000..d2fa4984a --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_planning_behaviours/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mdr_planning_behaviours) + +find_package(catkin REQUIRED COMPONENTS + rospy + roslint + smach + mas_execution_manager + mdr_rosplan_interface +) + +roslint_python() +catkin_python_setup() + +catkin_package( + CATKIN_DEPENDS + rospy + smach + mas_execution_manager + mdr_rosplan_interface +) + +include_directories( + ${catkin_INCLUDE_DIRS} +) diff --git a/mdr_planning/mdr_behaviours/mdr_planning_behaviours/package.xml b/mdr_planning/mdr_behaviours/mdr_planning_behaviours/package.xml new file mode 100644 index 000000000..bd567a5d2 --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_planning_behaviours/package.xml @@ -0,0 +1,25 @@ + + + mdr_planning_behaviours + 1.2.0 + A package for common planning-related robot behaviours + + Alex Mitrevski + MAS robotics + + GPLv3 + + catkin + rospy + roslint + smach + mas_execution_manager + mdr_rosplan_interface + + rospy + smach + mas_execution_manager + mdr_rosplan_interface + + + diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/__init__.py b/mdr_planning/mdr_behaviours/mdr_planning_behaviours/ros/src/mdr_planning_behaviours/__init__.py similarity index 100% rename from mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/__init__.py rename to mdr_planning/mdr_behaviours/mdr_planning_behaviours/ros/src/mdr_planning_behaviours/__init__.py diff --git a/mdr_planning/mdr_behaviours/mdr_planning_behaviours/ros/src/mdr_planning_behaviours/dispatch_plan.py b/mdr_planning/mdr_behaviours/mdr_planning_behaviours/ros/src/mdr_planning_behaviours/dispatch_plan.py new file mode 100644 index 000000000..180055acb --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_planning_behaviours/ros/src/mdr_planning_behaviours/dispatch_plan.py @@ -0,0 +1,19 @@ +from mas_execution_manager.scenario_state_base import ScenarioStateBase +from mdr_rosplan_interface.planner_interface import PlannerInterface + +class DispatchPlan(ScenarioStateBase): + def __init__(self, save_sm_state=False, **kwargs): + ScenarioStateBase.__init__(self, 'dispatch_plan', + save_sm_state=save_sm_state, + outcomes=['succeeded', 'failed']) + self.sm_id = kwargs.get('sm_id', '') + self.state_name = kwargs.get('state_name', 'dispatch_plan') + self.planner_interface = PlannerInterface() + + def execute(self, userdata): + dispatching = self.start_plan_dispatch.plan() + if not dispatching: + return 'failed' + + # TODO: monitor the plan + return 'succeeded' diff --git a/mdr_planning/mdr_behaviours/mdr_planning_behaviours/ros/src/mdr_planning_behaviours/invoke_planner.py b/mdr_planning/mdr_behaviours/mdr_planning_behaviours/ros/src/mdr_planning_behaviours/invoke_planner.py new file mode 100644 index 000000000..f7ec65589 --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_planning_behaviours/ros/src/mdr_planning_behaviours/invoke_planner.py @@ -0,0 +1,17 @@ +from mas_execution_manager.scenario_state_base import ScenarioStateBase +from mdr_rosplan_interface.planner_interface import PlannerInterface + +class InvokePlanner(ScenarioStateBase): + def __init__(self, save_sm_state=False, **kwargs): + ScenarioStateBase.__init__(self, 'invoke_planner', + save_sm_state=save_sm_state, + outcomes=['succeeded', 'failed']) + self.sm_id = kwargs.get('sm_id', '') + self.state_name = kwargs.get('state_name', 'invoke_planner') + self.planner_interface = PlannerInterface() + + def execute(self, userdata): + planner_called = self.planner_interface.plan() + if planner_called: + return 'succeeded' + return 'failed' diff --git a/mdr_planning/mdr_behaviours/mdr_planning_behaviours/setup.py b/mdr_planning/mdr_behaviours/mdr_planning_behaviours/setup.py new file mode 100644 index 000000000..12161e1ef --- /dev/null +++ b/mdr_planning/mdr_behaviours/mdr_planning_behaviours/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['mdr_planning_behaviours'], + package_dir={'mdr_planning_behaviours': 'ros/src/mdr_planning_behaviours'} +) + +setup(**d) diff --git a/mdr_planning/mdr_knowledge_base/CMakeLists.txt b/mdr_planning/mdr_knowledge_base/CMakeLists.txt index 314d0b4a0..1f87b2771 100644 --- a/mdr_planning/mdr_knowledge_base/CMakeLists.txt +++ b/mdr_planning/mdr_knowledge_base/CMakeLists.txt @@ -7,10 +7,11 @@ find_package(catkin REQUIRED COMPONENTS roslint ) -catkin_package() +catkin_package( + CATKIN_DEPENDS + rospy +) include_directories( ${catkin_INCLUDE_DIRS} ) - -roslint_python() diff --git a/mdr_planning/mdr_rosplan_interface/CMakeLists.txt b/mdr_planning/mdr_rosplan_interface/CMakeLists.txt index ebfddac05..eb6e4db44 100644 --- a/mdr_planning/mdr_rosplan_interface/CMakeLists.txt +++ b/mdr_planning/mdr_rosplan_interface/CMakeLists.txt @@ -5,6 +5,7 @@ find_package(catkin REQUIRED COMPONENTS rosplan_knowledge_msgs rospy roslint + mas_knowledge_base ) catkin_python_setup() diff --git a/mdr_planning/mdr_rosplan_interface/README.md b/mdr_planning/mdr_rosplan_interface/README.md index 0920feb8b..7564e7d93 100644 --- a/mdr_planning/mdr_rosplan_interface/README.md +++ b/mdr_planning/mdr_rosplan_interface/README.md @@ -6,7 +6,7 @@ A package for interacting with the ROSPlan planning system. ### action_client_base -A base implementation of an action client class able to (i) respond to action dispatch messages sent by a ROSPlan dispatcher and (ii) update the knowledge base based on an executed action. +A base implementation of an action client class (`ActionClientBase`) able to (i) respond to action dispatch messages sent by a ROSPlan dispatcher and (ii) update the knowledge base based on an executed action. The base class defines the following methods: * `call_action`: Callback method called when an action is dispatched. Only needs to listen to request for a particular action; all other requests should be ignored. @@ -16,6 +16,20 @@ The base class defines the following methods: Action-specific implementations need to override `call_action`, `get_action_message`, and `update_knowledge_base`; `send_action_feedback` is already implemented in the base class. Example implementations of action clients can be found in the `mdr_actions` package (the [pickup action client](../mdr_actions/mdr_manipulation_actions/mdr_pickup_action/ros/scripts/pickup_client) is a representative example). +### planner_interface + +An interface (`PlannerInterface`) for interacting with a planner. The interface exposes the following methods: +* `add_plan_goals`: Registers a list of ground predicates as planning goals.s +* `remove_plan_goals`: Removes a list of ground predicates from the list of planning goals. +* `plan`: Generates a problem from the current state of the knowledge base and invokes the planner. +* `start_plan_dispatch`: Parses a previously generated plan and invokes the plan dispatcher. + +The interface contains an instance of the `DomesticKBInterface` class in [`mas_knowledge_base`](https://github.com/b-it-bots/mas_knowledge_base) for interacting with the knowledge base. In addition, the following parameters need to be exposed on the ROS parameters server: +* `/planner/problem_generation_srv`: Name of a service that generates a planner problem +* `/planner/planner_srv`: Name of a service that invokes a planner +* `/planner/plan_parsing_srv`: Name of a service that parses a generated plan +* `/planner/plan_dispatch_srv`: Name of a service that invokes a plan dispatcher + ## Executable scripts ### init_planning_problem @@ -83,4 +97,9 @@ mdr_rosplan_interface |____mdr_rosplan_interface | __init__.py |____action_client_base.py -``` \ No newline at end of file +``` + +## Dependencies + +* `ROSPlan` +* `mas_knowledge_base` diff --git a/mdr_planning/mdr_rosplan_interface/config/default_domain.pddl b/mdr_planning/mdr_rosplan_interface/config/default_domain.pddl new file mode 100644 index 000000000..d33d086f9 --- /dev/null +++ b/mdr_planning/mdr_rosplan_interface/config/default_domain.pddl @@ -0,0 +1,115 @@ +(define (domain default-domestic-domain) + + (:requirements :strips :typing :fluents :disjunctive-preconditions :durative-actions) + + (:types + waypoint + object + category + robot + door + plane + ) + + (:predicates + (robot_name ?bot - robot) + (object_category ?obj - object ?cat - category) + (robot_at ?bot - robot ?wp - waypoint) + (door_at ?door - door ?wp - waypoint) + (object_at ?obj - object ?wp - waypoint) + (plane_at ?plane - plane ?wp - waypoint) + (door_open ?door - door) + (belongs_to ?plane - plane ?obj - object) + (unexplored ?plane - plane) + (explored ?plane - plane) + (on ?obj - object ?plane - plane) + (in ?obj - object ?source - object) + (holding ?bot - robot ?obj - object) + (empty_gripper ?bot - robot) + ) + + (:durative-action move_base + :parameters (?bot - robot ?from ?to - waypoint) + :duration ( = ?duration 10) + :condition (and + (at start (robot_at ?bot ?from)) + ) + :effect (and + (at start (not (robot_at ?bot ?from))) + (at end (robot_at ?bot ?to)) + ) + ) + + (:durative-action open_cupboard + :parameters (?cupboard - door ?bot - robot ?c - waypoint) + :duration ( = ?duration 10) + :condition (and + (at start (door_at ?cupboard ?c)) + (at start (robot_at ?bot ?c)) + ) + :effect (and + (at end (door_open ?cupboard)) + ) + ) + + (:durative-action perceive_plane + :parameters (?plane - plane ?bot - robot ?wp - waypoint) + :duration ( = ?duration 10) + :condition (and + (at start (robot_at ?bot ?wp)) + (at start (plane_at ?plane ?wp)) + (at start (unexplored ?plane)) + ) + :effect (and + (at start (not (unexplored ?plane))) + (at end (explored ?plane)) + ) + ) + + (:durative-action pickup + :parameters (?obj - object ?plane - plane ?bot - robot ?wp - waypoint) + :duration ( = ?duration 10) + :condition (and + (at start (robot_at ?bot ?wp)) + (at start (plane_at ?plane ?wp)) + (at start (explored ?plane)) + (at start (on ?obj ?plane)) + (at start (empty_gripper ?bot)) + ) + :effect (and + (at start (not (on ?obj ?plane))) + (at start (not (empty_gripper ?bot))) + (at end (holding ?bot ?obj)) + ) + ) + + (:durative-action place + :parameters (?obj - object ?plane - plane ?bot - robot ?wp - waypoint) + :duration ( = ?duration 10) + :condition (and + (at start (robot_at ?bot ?wp)) + (at start (plane_at ?plane ?wp)) + (at start (holding ?bot ?obj)) + ) + :effect (and + (at start (not (holding ?bot ?obj))) + (at start (empty_gripper ?bot)) + (at end (on ?obj ?plane)) + ) + ) + + (:durative-action throw + :parameters (?obj - object ?dest_obj - object ?bot - robot ?wp - waypoint) + :duration ( = ?duration 10) + :condition (and + (at start (robot_at ?bot ?wp)) + (at start (object_at ?dest_obj ?wp)) + (at start (holding ?bot ?obj)) + ) + :effect (and + (at start (not (holding ?bot ?obj))) + (at start (empty_gripper ?bot)) + (at end (in ?obj ?dest_obj)) + ) + ) +) diff --git a/mdr_planning/mdr_rosplan_interface/package.xml b/mdr_planning/mdr_rosplan_interface/package.xml index 4325c92b2..2532a7ccd 100644 --- a/mdr_planning/mdr_rosplan_interface/package.xml +++ b/mdr_planning/mdr_rosplan_interface/package.xml @@ -9,9 +9,11 @@ MAS robotics catkin - rosplan_knowledge_msgs rospy + rosplan_knowledge_msgs + mas_knowledge_base - rosplan_knowledge_msgs rospy + rosplan_knowledge_msgs + mas_knowledge_base diff --git a/mdr_planning/mdr_rosplan_interface/ros/launch/clear_message_store.launch b/mdr_planning/mdr_rosplan_interface/ros/launch/clear_message_store.launch new file mode 100644 index 000000000..ed0b9b88d --- /dev/null +++ b/mdr_planning/mdr_rosplan_interface/ros/launch/clear_message_store.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch b/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch index 557082229..073a01f1e 100644 --- a/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch +++ b/mdr_planning/mdr_rosplan_interface/ros/launch/rosplan.launch @@ -3,20 +3,28 @@ - + - - + + - + + + + + + + + + - + @@ -27,17 +35,48 @@ - + + + + + + + + + + - + + + + - - - + + + + + + + + + + + + + + + + + + + + + + diff --git a/mdr_planning/mdr_rosplan_interface/ros/scripts/clear_message_store b/mdr_planning/mdr_rosplan_interface/ros/scripts/clear_message_store new file mode 100755 index 000000000..660724b9b --- /dev/null +++ b/mdr_planning/mdr_rosplan_interface/ros/scripts/clear_message_store @@ -0,0 +1,27 @@ +#!/usr/bin/env python + +import rospy +import pymongo as pm + +class ClearMessageStore(object): + def __init__(self): + self.db_name = rospy.get_param('~db_name', 'message_store') + self.collection_name = rospy.get_param('~collection_name', 'message_store') + self.db_port = rospy.get_param('~db_port', 27018) + self.__clear_message_store() + + def __clear_message_store(self): + try: + client = pm.MongoClient(port=self.db_port) + database = client[self.db_name] + collection = database[self.collection_name] + collection.delete_many({}) + except Exception as exc: + rospy.logerr(str(exc)) + +if __name__ == '__main__': + rospy.init_node('clear_message_store') + try: + ClearMessageStore() + except rospy.ROSInterruptException: + pass diff --git a/mdr_planning/mdr_rosplan_interface/ros/src/mdr_rosplan_interface/action_client_base.py b/mdr_planning/mdr_rosplan_interface/ros/src/mdr_rosplan_interface/action_client_base.py index dd79fc045..4f49656a4 100644 --- a/mdr_planning/mdr_rosplan_interface/ros/src/mdr_rosplan_interface/action_client_base.py +++ b/mdr_planning/mdr_rosplan_interface/ros/src/mdr_rosplan_interface/action_client_base.py @@ -4,54 +4,86 @@ import rospy import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import rosplan_knowledge_msgs.srv as rosplan_srvs import diagnostic_msgs.msg as diag_msgs -from mongodb_store.message_store import MessageStoreProxy +from mas_knowledge_base.domestic_kb_interface import DomesticKBInterface class ActionClientBase(object): + '''An abstract base class for knowledge-enabled action clients. + + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de + + ''' def __init__(self): self.action_success_msg = 'action achieved' self.action_failure_msg = 'action failed' + + # unique action ID self.action_id = -1 + # name of the robot on which the client is spawned self.robot_name = None + # name of the action (converted to lowercase) self.action_name = rospy.get_param('~action_name', '') self.action_name = self.action_name.lower() + # name of the action server self.action_server_name = rospy.get_param('~server_name', '') - self.action_timeout = rospy.get_param('~action_timeout', 15.) - self.knowledge_update_client = rospy.ServiceProxy('knowledge_update_service', - rosplan_srvs.KnowledgeUpdateService) - - self.attribute_fetching_client = rospy.ServiceProxy('attribute_fetching_service', - rosplan_srvs.GetAttributeService) + # timeout for action calls + self.action_timeout = rospy.get_param('~action_timeout', 15.) - self.msg_store_client = MessageStoreProxy() + # knowledge base interface instance + self.kb_interface = DomesticKBInterface() + # subscriber for dispatched actions rospy.Subscriber('action_dispatch_topic', plan_dispatch_msgs.ActionDispatch, self.call_action) + # action feedback publisher self.feedback_pub = rospy.Publisher('action_feedback_topic', plan_dispatch_msgs.ActionFeedback, queue_size=1) @abc.abstractmethod def call_action(self, msg): + '''Abstract callback for the dispatched action subscriber. + Only reacts to request to "self.action_name"; ignores all other requests. + + Keyword arguments: + msg -- a rosplan_dispatch_msgs.msg.ActionDispatch instance + + ''' pass @abc.abstractmethod def get_action_message(self, rosplan_action_msg): + '''Abstract method for converting the message to an action request. + Returns an actionlib goal instance for the action. + + Keyword arguments: + rosplan_action_msg -- a rosplan_dispatch_msgs.msg.ActionDispatch instance + + ''' return None @abc.abstractmethod - def update_knowledge_base(self, source_location, destination_location): + def update_knowledge_base(self): + '''Abstract method for updating the knowledge base after + the successful completion of an action. + ''' pass - @abc.abstractmethod def send_action_feedback(self, success): + '''Publishes a rosplan_dispatch_msgs.msg.ActionFeedback message + based on the result of the action execution. + + Keyword arguments: + success -- a Boolean indicating whether the action was successfully executed + + ''' msg = plan_dispatch_msgs.ActionFeedback() msg.action_id = self.action_id if success: diff --git a/mdr_planning/mdr_rosplan_interface/ros/src/mdr_rosplan_interface/planner_interface.py b/mdr_planning/mdr_rosplan_interface/ros/src/mdr_rosplan_interface/planner_interface.py new file mode 100644 index 000000000..b9f10dfdf --- /dev/null +++ b/mdr_planning/mdr_rosplan_interface/ros/src/mdr_rosplan_interface/planner_interface.py @@ -0,0 +1,144 @@ +import rospy +from std_srvs.srv import Empty +import rosplan_dispatch_msgs.msg as plan_dispatch_msgs +from mas_knowledge_base.domestic_kb_interface import DomesticKBInterface + +class PlannerInterface(object): + '''An interface providing functionalities for interacting with a planner. + + The interface expects four planner services to be exposed, whose names should be + made available to the parameter server as follows: + * /planner/problem_generation_srv: Name of a service that generates a planner problem + * /planner/planner_srv: Name of a service that invokes a planner + * /planner/plan_parsing_srv: Name of a service that parses a generated plan + * /planner/plan_dispatch_srv: Name of a service that invokes a plan dispatcher + + @author Alex Mitrevski + @contact aleksandar.mitrevski@h-brs.de + + ''' + def __init__(self): + self.problem_generation_proxy = None + self.planner_proxy = None + self.plan_parsing_proxy = None + self.plan_dispatch_proxy = None + self.action_dispatch_topic = rospy.get_param('/planner/action_dispatch_topic', + '/kcl_rosplan/action_dispatch') + self.action_feedback_topic = rospy.get_param('/planner/action_feedback_topic', + '/kcl_rosplan/action_feedback') + self.kb_interface = DomesticKBInterface() + + problem_generation_srv = rospy.get_param('/planner/problem_generation_srv', + '/rosplan_problem_interface/problem_generation_server') + try: + rospy.wait_for_service(problem_generation_srv, 5.) + self.problem_generation_proxy = rospy.ServiceProxy(problem_generation_srv, Empty) + except (rospy.ServiceException, rospy.ROSException): + rospy.logerr('The service %s does not appear to exist.', problem_generation_srv) + + planner_srv = rospy.get_param('/planner/planner_srv', + '/rosplan_planner_interface/planning_server') + try: + rospy.wait_for_service(planner_srv, 5.) + self.planner_proxy = rospy.ServiceProxy(planner_srv, Empty) + except (rospy.ServiceException, rospy.ROSException): + rospy.logerr('The service %s does not appear to exist.', planner_srv) + + plan_parsing_srv = rospy.get_param('/planner/plan_parsing_srv', + '/rosplan_parsing_interface/parse_plan') + try: + rospy.wait_for_service(plan_parsing_srv, 5.) + self.plan_parsing_proxy = rospy.ServiceProxy(plan_parsing_srv, Empty) + except (rospy.ServiceException, rospy.ROSException): + rospy.logerr('The service %s does not appear to exist.', plan_parsing_srv) + + plan_dispatch_srv = rospy.get_param('/planner/plan_dispatch_srv', + '/rosplan_plan_dispatcher/dispatch_plan') + try: + rospy.wait_for_service(plan_dispatch_srv, 5.) + self.plan_dispatch_proxy = rospy.ServiceProxy(plan_dispatch_srv, Empty) + except (rospy.ServiceException, rospy.ROSException): + rospy.logerr('The service %s does not appear to exist.', plan_dispatch_srv) + + self.current_action = '' + self.executing = False + self.action_dispatch_sub = rospy.Subscriber(self.action_dispatch_topic, + plan_dispatch_msgs.ActionDispatch, + self.get_dispatched_action) + self.action_feedback_sub = rospy.Subscriber(self.action_feedback_topic, + plan_dispatch_msgs.ActionFeedback, + self.get_action_feedback) + + def add_plan_goals(self, goals): + '''Registers the ground predicates in the given list as planning goals. + + Keyword arguments: + @param goal_list -- a list in which each entry is a tuple of the form + (predicate, [(variable, value), ...]), where + "predicate" is the predicate name and the + (variable, value) tuples are the variable values + + ''' + self.kb_interface.insert_goals(goals) + + def remove_plan_goals(self, goals): + '''Removes the ground predicates in the given list from the list of planning goals. + + Keyword arguments: + @param goal_list -- a list in which each entry is a tuple of the form + (predicate, [(variable, value), ...]), where + "predicate" is the predicate name and the + (variable, value) tuples are the variable values + + ''' + self.kb_interface.remove_goals(goals) + + def plan(self): + '''Generates a problem file from the current state of the knowledge base + and invokes the planner. + ''' + rospy.loginfo('[planner_interface] Generating a problem file') + try: + self.problem_generation_proxy() + except rospy.ServiceException as exc: + rospy.logerr('An error occured while generating a problem file: ' + str(exc)) + return False + + rospy.loginfo('[planner_interface] Invoking the planner') + try: + self.planner_proxy() + except rospy.ServiceException as exc: + rospy.logerr('An error occured while planning: ' + str(exc)) + return False + return True + + def start_plan_dispatch(self): + '''Parses a previously generated plan and starts dispatching the plan. + ''' + rospy.loginfo('[planner_interface] Parsing the plan') + try: + self.plan_parsing_proxy() + except rospy.ServiceException as exc: + rospy.logerr('An error occured while parsing the plan: ' + str(exc)) + return False + + rospy.loginfo('[planner_interface] Invoking the plan dispatcher') + try: + self.plan_dispatch_proxy() + except rospy.ServiceException as exc: + rospy.logerr('An error occured while dispatching the plan: ' + str(exc)) + return False + + self.executing = True + return True + + def get_current_action(self): + return self.current_action + + def get_dispatched_action(self, msg): + self.current_action = msg.name + + def get_action_feedback(self, msg): + if msg.information and msg.information[0].key == 'action_name' and \ + msg.information[0].value == self.current_action: + self.executing = msg.status != 'action achieved' diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/CMakeLists.txt b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/CMakeLists.txt index 4e4cda288..6072a122e 100644 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/CMakeLists.txt +++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/CMakeLists.txt @@ -2,35 +2,18 @@ cmake_minimum_required(VERSION 2.8.3) project(mdr_demo_patrol) find_package(catkin REQUIRED COMPONENTS - roslint - rospy - smach_ros - smach - actionlib - mongodb_store - mdr_rosplan_interface - mas_execution_manager - mdr_move_base_action mdr_move_arm_action + mdr_move_base_action + mdr_navigation_behaviours ) catkin_package( CATKIN_DEPENDS - roslint - rospy - smach_ros - smach - actionlib - mongodb_store - mdr_rosplan_interface - mas_execution_manager - mdr_move_base_action mdr_move_arm_action + mdr_move_base_action + mdr_navigation_behaviours ) -catkin_python_setup() -roslint_python() - install(DIRECTORY ros/launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ros/launch ) diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/config/patrol_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/config/patrol_sm.yaml index 8b3aa0ec8..57ec6da66 100644 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/config/patrol_sm.yaml +++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/config/patrol_sm.yaml @@ -7,7 +7,7 @@ outcomes: [DONE, TIMEOUT, FAILED] state_descriptions: - state: name: GO_TO - state_module_name: mdr_demo_patrol.scenario_states.move_base + state_module_name: mdr_navigation_behaviours.move_base state_class_name: MoveBase transitions: - transition: diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/package.xml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/package.xml index 765995100..18414a945 100644 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/package.xml +++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/package.xml @@ -10,29 +10,13 @@ catkin - rospy - roslint - smach - smach_ros - actionlib - mongodb_store - mdr_rosplan_interface - mas_execution_manager - mdr_move_base_action mdr_move_arm_action + mdr_move_base_action + mdr_navigation_behaviours - rospy - roslint - smach - smach_ros - actionlib - mongodb_store - rosplan_knowledge_base - rosplan_planning_system - mdr_rosplan_interface - mas_execution_manager - mdr_move_base_action mdr_move_arm_action + mdr_move_base_action + mdr_navigation_behaviours roslaunch diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/CMakeLists.txt b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/CMakeLists.txt index fac562043..48a87d8eb 100644 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/CMakeLists.txt +++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/CMakeLists.txt @@ -2,41 +2,32 @@ cmake_minimum_required(VERSION 2.8.3) project(mdr_demo_simple_pick_and_place) find_package(catkin REQUIRED COMPONENTS - roslint rospy - smach_ros - smach - actionlib - mongodb_store - mdr_rosplan_interface - mas_execution_manager - mdr_move_base_action mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action mdr_pickup_action mdr_place_action - mdr_perceive_plane_action + mas_execution_manager + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours ) catkin_package( CATKIN_DEPENDS - roslint rospy - smach_ros - smach - actionlib - mongodb_store - mdr_rosplan_interface - mas_execution_manager - mdr_move_base_action mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action mdr_pickup_action mdr_place_action - mdr_perceive_plane_action + mas_execution_manager + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours ) -catkin_python_setup() -roslint_python() - install(DIRECTORY ros/launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ros/launch ) diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/config/pick_and_place_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/config/pick_and_place_sm.yaml index ea7e4f822..fb9e67142 100644 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/config/pick_and_place_sm.yaml +++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/config/pick_and_place_sm.yaml @@ -7,7 +7,7 @@ outcomes: [DONE, FAILED, TIMEOUT] state_descriptions: - state: name: GO_TO_TABLE - state_module_name: mdr_demo_simple_pick_and_place.scenario_states.move_base + state_module_name: mdr_navigation_behaviours.move_base state_class_name: MoveBase transitions: - transition: @@ -28,7 +28,7 @@ state_descriptions: value: 3 - state: name: PERCEIVE_TABLE - state_module_name: mdr_demo_simple_pick_and_place.scenario_states.perceive_planes + state_module_name: mdr_perception_behaviours.perceive_planes state_class_name: PerceivePlanes transitions: - transition: @@ -49,8 +49,8 @@ state_descriptions: value: table - state: name: PICK_OBJECT - state_module_name: mdr_demo_simple_pick_and_place.scenario_states.pick - state_class_name: Pick + state_module_name: mdr_manipulation_behaviours.pick_closest_from_surface + state_class_name: PickClosestFromSurface transitions: - transition: name: succeeded @@ -68,9 +68,12 @@ state_descriptions: - argument: name: number_of_retries value: 3 + - argument: + name: picking_surface_prefix + value: table - state: name: FIND_OBJECTS_BEFORE_PICKING - state_module_name: mdr_demo_simple_pick_and_place.scenario_states.perceive_planes + state_module_name: mdr_perception_behaviours.perceive_planes state_class_name: PerceivePlanes transitions: - transition: @@ -91,11 +94,11 @@ state_descriptions: value: table - state: name: PLACE_OBJECT - state_module_name: mdr_demo_simple_pick_and_place.scenario_states.place + state_module_name: mdr_manipulation_behaviours.place state_class_name: Place transitions: - transition: - name: pick_new_object + name: succeeded state: PERCEIVE_TABLE - transition: name: failed @@ -107,3 +110,6 @@ state_descriptions: - argument: name: number_of_retries value: 3 + - argument: + name: placing_surface_prefix + value: table diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/package.xml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/package.xml index 3fd4b2660..ab5b77c25 100644 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/package.xml +++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/package.xml @@ -11,37 +11,24 @@ catkin rospy - roslint - smach - smach_ros - actionlib - mongodb_store - - mdr_rosplan_interface - mas_execution_manager - mdr_move_base_action mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action mdr_pickup_action mdr_place_action - mdr_perceive_plane_action + mas_execution_manager + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours rospy - roslint - smach - smach_ros - actionlib - - mongodb_store - rosplan_knowledge_base - rosplan_planning_system - - mdr_rosplan_interface - mas_execution_manager - mdr_move_base_action mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action mdr_pickup_action mdr_place_action - mdr_perceive_plane_action - - roslaunch + mas_execution_manager + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/move_base.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/move_base.py deleted file mode 100644 index 15ccd830f..000000000 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/move_base.py +++ /dev/null @@ -1,102 +0,0 @@ -import time -import rospy -import actionlib -from std_msgs.msg import String -from geometry_msgs.msg import PoseStamped - -import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import rosplan_knowledge_msgs.srv as rosplan_srvs -import diagnostic_msgs.msg as diag_msgs - -from mdr_move_base_action.msg import MoveBaseAction, MoveBaseGoal -from mas_execution_manager.scenario_state_base import ScenarioStateBase - -class MoveBase(ScenarioStateBase): - def __init__(self, save_sm_state=False, **kwargs): - ScenarioStateBase.__init__(self, 'move_base', - save_sm_state=save_sm_state, - outcomes=['succeeded', 'failed', 'failed_after_retrying']) - self.sm_id = kwargs.get('sm_id', 'mdr_demo_simple_pick_and_place') - self.state_name = kwargs.get('state_name', 'move_base') - self.move_base_server = kwargs.get('move_base_server', 'move_base_server') - self.destination_locations = list(kwargs.get('destination_locations', list())) - self.timeout = kwargs.get('timeout', 120.) - self.save_sm_state = save_sm_state - - self.number_of_retries = kwargs.get('number_of_retries', 0) - self.retry_count = 0 - - self.move_base_client = actionlib.SimpleActionClient(self.move_base_server, - MoveBaseAction) - self.move_base_client.wait_for_server() - - def execute(self, userdata): - if self.save_sm_state: - self.save_current_state() - - # if the current location of the robot was not specified - # in the request, we query this information from the knowledge base - original_location = '' - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = 'robot_at' - result = self.attribute_fetching_client(request) - for item in result.attributes: - if not item.is_negative: - for param in item.values: - if param.key == 'bot' and param.value != self.robot_name: - break - - if param.key == 'wp': - original_location = param.value - break - break - - for destination_location in self.destination_locations: - dispatch_msg = self.get_dispatch_msg(original_location, - destination_location) - - rospy.loginfo('Sending the base to %s' % destination_location) - self.say('Going to ' + destination_location) - self.action_dispatch_pub.publish(dispatch_msg) - - self.executing = True - self.succeeded = False - start_time = time.time() - duration = 0. - while self.executing and duration < self.timeout: - rospy.sleep(0.1) - duration = time.time() - start_time - - if self.succeeded: - rospy.loginfo('Successfully reached %s' % destination_location) - original_location = destination_location - else: - rospy.logerr('Could not reach %s' % destination_location) - self.say('Could not reach ' + destination_location) - if self.retry_count == self.number_of_retries: - self.say('Aborting operation') - return 'failed_after_retrying' - self.retry_count += 1 - return 'failed' - return 'succeeded' - - def get_dispatch_msg(self, original_location, destination_location): - dispatch_msg = plan_dispatch_msgs.ActionDispatch() - dispatch_msg.name = self.action_name - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'from' - arg_msg.value = original_location - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'to' - arg_msg.value = destination_location - dispatch_msg.parameters.append(arg_msg) - - return dispatch_msg diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/perceive_planes.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/perceive_planes.py deleted file mode 100644 index 910388ce8..000000000 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/perceive_planes.py +++ /dev/null @@ -1,68 +0,0 @@ -import time -import rospy -from std_msgs.msg import String - -import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import diagnostic_msgs.msg as diag_msgs - -from mas_execution_manager.scenario_state_base import ScenarioStateBase - -class PerceivePlanes(ScenarioStateBase): - def __init__(self, save_sm_state=False, **kwargs): - ScenarioStateBase.__init__(self, 'perceive_plane', - save_sm_state=save_sm_state, - outcomes=['succeeded', 'failed', 'failed_after_retrying']) - self.sm_id = kwargs.get('sm_id', 'mdr_demo_simple_pick_and_place') - self.state_name = kwargs.get('state_name', 'perceive_planes') - self.timeout = kwargs.get('timeout', 120.) - self.plane_prefix = kwargs.get('plane_prefix', 0) - - self.number_of_retries = kwargs.get('number_of_retries', 0) - self.retry_count = 0 - - def execute(self, userdata): - if self.save_sm_state: - self.save_current_state() - - dispatch_msg = self.get_dispatch_msg(self.plane_prefix) - rospy.loginfo('Perceiving plane %s' % self.plane_prefix) - self.say('Perceiving plane ' + self.plane_prefix) - self.action_dispatch_pub.publish(dispatch_msg) - - self.executing = True - self.succeeded = False - start_time = time.time() - duration = 0. - while self.executing and duration < self.timeout: - rospy.sleep(0.1) - duration = time.time() - start_time - - if self.succeeded: - rospy.loginfo('%s perceived successfully' % self.plane_prefix) - return 'succeeded' - - rospy.loginfo('Could not perceive %s' % self.plane_prefix) - self.say('Could not perceive ' + self.plane_prefix) - if self.retry_count == self.number_of_retries: - rospy.loginfo('Failed to perceive %s' % self.plane_prefix) - self.say('Aborting operation') - return 'failed_after_retrying' - rospy.loginfo('Retrying to perceive %s' % self.plane_prefix) - self.retry_count += 1 - return 'failed' - - def get_dispatch_msg(self, plane_name): - dispatch_msg = plan_dispatch_msgs.ActionDispatch() - dispatch_msg.name = self.action_name - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'plane' - arg_msg.value = plane_name - dispatch_msg.parameters.append(arg_msg) - - return dispatch_msg diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/pick.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/pick.py deleted file mode 100644 index 78be7dec1..000000000 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_simple_pick_and_place/ros/src/mdr_demo_simple_pick_and_place/scenario_states/pick.py +++ /dev/null @@ -1,167 +0,0 @@ -import time -import numpy as np -import rospy -from std_msgs.msg import String -from tf import TransformListener - -from geometry_msgs.msg import PointStamped - -import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import rosplan_knowledge_msgs.srv as rosplan_srvs -import diagnostic_msgs.msg as diag_msgs - -from mcr_perception_msgs.msg import Object -from mas_execution_manager.scenario_state_base import ScenarioStateBase - -class Pick(ScenarioStateBase): - def __init__(self, save_sm_state=False, **kwargs): - ScenarioStateBase.__init__(self, 'pickup', - save_sm_state=save_sm_state, - output_keys=['grasped_object'], - outcomes=['succeeded', 'failed', - 'failed_after_retrying', - 'find_objects_before_picking']) - self.sm_id = kwargs.get('sm_id', 'mdr_demo_simple_pick_and_place') - self.state_name = kwargs.get('state_name', 'pick') - self.timeout = kwargs.get('timeout', 120.) - self.tf_listener = TransformListener() - - self.number_of_retries = kwargs.get('number_of_retries', 0) - self.retry_count = 0 - - def execute(self, userdata): - if self.save_sm_state: - self.save_current_state() - - surface_objects = self.get_surface_objects(surface_prefix='table') - object_poses = self.get_object_poses(surface_objects) - surface, obj_to_grasp_idx = self.select_object_for_grasping(object_poses) - obj_to_grasp = '' - if obj_to_grasp_idx != -1: - obj_to_grasp = surface_objects[surface][obj_to_grasp_idx] - else: - rospy.logerr('Could not find an object to grasp') - self.say('Could not find an object to grasp') - return 'find_objects_before_picking' - - dispatch_msg = self.get_dispatch_msg(obj_to_grasp, surface) - rospy.loginfo('Picking %s from %s' % (obj_to_grasp, surface)) - self.say('Picking ' + obj_to_grasp + ' from ' + surface) - self.action_dispatch_pub.publish(dispatch_msg) - - self.executing = True - self.succeeded = False - start_time = time.time() - duration = 0. - while self.executing and duration < self.timeout: - rospy.sleep(0.1) - duration = time.time() - start_time - - if self.succeeded: - rospy.loginfo('%s grasped successfully' % obj_to_grasp) - self.say('Successfully grasped ' + obj_to_grasp) - userdata.grasped_object = obj_to_grasp - return 'succeeded' - - rospy.loginfo('Could not grasp %s' % obj_to_grasp) - self.say('Could not grasp ' + obj_to_grasp) - if self.retry_count == self.number_of_retries: - rospy.loginfo('Failed to grasp %s' % obj_to_grasp) - self.say('Aborting operation') - return 'failed_after_retrying' - rospy.loginfo('Retrying to grasp %s' % obj_to_grasp) - self.retry_count += 1 - return 'failed' - - def get_surface_objects(self, surface_prefix='table'): - surface_objects = dict() - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = 'on' - result = self.attribute_fetching_client(request) - for item in result.attributes: - object_on_desired_surface = False - object_name = '' - surface_name = '' - if not item.is_negative: - for param in item.values: - if param.key == 'plane' and param.value.find(surface_prefix) != -1: - object_on_desired_surface = True - surface_name = param.value - if surface_name not in surface_objects: - surface_objects[surface_name] = list() - elif param.key == 'obj': - object_name = param.value - if object_on_desired_surface: - surface_objects[surface_name].append(object_name) - return surface_objects - - def get_object_poses(self, surface_objects): - object_poses = dict() - for surface, objects in surface_objects.items(): - object_poses[surface] = list() - for obj_name in objects: - try: - obj = self.msg_store_client.query_named(obj_name, Object._type)[0] - object_poses[surface].append(obj.pose) - except: - rospy.logerr('Error retriving knowledge about %s', obj_name) - pass - return object_poses - - def select_object_for_grasping(self, object_poses): - '''Returns the index of the object whose distance is closest to the robot - ''' - object_surfaces = list() - distances = dict() - robot_position = np.zeros(3) - for surface, poses in object_poses.items(): - object_surfaces.append(surface) - distances[surface] = list() - for pose in poses: - base_link_pose = self.tf_listener.transformPose('base_link', pose) - distances[surface].append(self.distance(robot_position, np.array([base_link_pose.pose.position.x, - base_link_pose.pose.position.y, - base_link_pose.pose.position.z]))) - - min_dist = 1e100 - min_dist_obj_idx = -1 - obj_surface = '' - for surface, distance_list in distances.items(): - if distance_list: - surface_min_dist = np.min(distance_list) - if surface_min_dist < min_dist: - min_dist = surface_min_dist - min_dist_obj_idx = np.argmin(distance_list) - obj_surface = surface - return obj_surface, min_dist_obj_idx - - def get_robot_pose(self, map_frame='map', base_link_frame='base_link'): - latest_tf_time = self.tf_listener.getLatestCommonTime(base_link_frame, map_frame) - position, quat_orientation = self.tf_listener.lookupTransform(base_link_frame, - map_frame, - latest_tf_time) - return position, quat_orientation - - def distance(self, point1, point2): - return np.linalg.norm(np.array(point1) - np.array(point2)) - - def get_dispatch_msg(self, obj_name, surface_name): - dispatch_msg = plan_dispatch_msgs.ActionDispatch() - dispatch_msg.name = self.action_name - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'obj' - arg_msg.value = obj_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'plane' - arg_msg.value = surface_name - dispatch_msg.parameters.append(arg_msg) - - return dispatch_msg diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/CMakeLists.txt b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/CMakeLists.txt index 65aabed88..3000e1afd 100644 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/CMakeLists.txt +++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/CMakeLists.txt @@ -2,41 +2,32 @@ cmake_minimum_required(VERSION 2.8.3) project(mdr_demo_throw_table_objects) find_package(catkin REQUIRED COMPONENTS - roslint rospy - smach_ros - smach - actionlib - mongodb_store - mdr_rosplan_interface - mas_execution_manager - mdr_move_base_action mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action mdr_pickup_action mdr_place_action - mdr_perceive_plane_action + mas_execution_manager + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours ) catkin_package( CATKIN_DEPENDS - roslint rospy - smach_ros - smach - actionlib - mongodb_store - mdr_rosplan_interface - mas_execution_manager - mdr_move_base_action mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action mdr_pickup_action mdr_place_action - mdr_perceive_plane_action + mas_execution_manager + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours ) -catkin_python_setup() -roslint_python() - install(DIRECTORY ros/launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ros/launch ) diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/config/throw_table_objects_sm.yaml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/config/throw_table_objects_sm.yaml index 37c9ea62f..ce173fbae 100644 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/config/throw_table_objects_sm.yaml +++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/config/throw_table_objects_sm.yaml @@ -7,7 +7,7 @@ outcomes: [DONE, TIMEOUT, FAILED] state_descriptions: - state: name: GO_TO_OBSERVATION_POSE - state_module_name: mdr_demo_throw_table_objects.scenario_states.move_base + state_module_name: mdr_navigation_behaviours.move_base state_class_name: MoveBase transitions: - transition: @@ -28,7 +28,7 @@ state_descriptions: value: 3 - state: name: PERCEIVE_TABLE - state_module_name: mdr_demo_throw_table_objects.scenario_states.perceive_planes + state_module_name: mdr_perception_behaviours.perceive_planes state_class_name: PerceivePlanes transitions: - transition: @@ -49,7 +49,7 @@ state_descriptions: value: table - state: name: GO_TO_TABLE - state_module_name: mdr_demo_throw_table_objects.scenario_states.move_base + state_module_name: mdr_navigation_behaviours.move_base state_class_name: MoveBase transitions: - transition: @@ -70,7 +70,7 @@ state_descriptions: value: 3 - state: name: PERCEIVE_TABLE_BEFORE_PICKING - state_module_name: mdr_demo_throw_table_objects.scenario_states.perceive_planes + state_module_name: mdr_perception_behaviours.perceive_planes state_class_name: PerceivePlanes transitions: - transition: @@ -91,8 +91,8 @@ state_descriptions: value: table - state: name: PICK_OBJECT - state_module_name: mdr_demo_throw_table_objects.scenario_states.pick - state_class_name: Pick + state_module_name: mdr_manipulation_behaviours.pick_closest_from_surface + state_class_name: PickClosestFromSurface transitions: - transition: name: succeeded @@ -110,9 +110,12 @@ state_descriptions: - argument: name: number_of_retries value: 3 + - argument: + name: picking_surface_prefix + value: table - state: name: GO_TO_TRASH_CAN - state_module_name: mdr_demo_throw_table_objects.scenario_states.move_base + state_module_name: mdr_navigation_behaviours.move_base state_class_name: MoveBase transitions: - transition: @@ -133,8 +136,8 @@ state_descriptions: value: 3 - state: name: THROW_OBJECT - state_module_name: mdr_demo_throw_table_objects.scenario_states.throw - state_class_name: Throw + state_module_name: mdr_manipulation_behaviours.throw_object_in + state_class_name: ThrowObjectIn transitions: - transition: name: succeeded diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/package.xml b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/package.xml index 9852c8599..2e7328bdb 100644 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/package.xml +++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/package.xml @@ -11,37 +11,24 @@ catkin rospy - roslint - smach - smach_ros - actionlib - mongodb_store - - mdr_rosplan_interface - mas_execution_manager - mdr_move_base_action mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action mdr_pickup_action mdr_place_action - mdr_perceive_plane_action + mas_execution_manager + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours rospy - roslint - smach - smach_ros - actionlib - - mongodb_store - rosplan_knowledge_base - rosplan_planning_system - - mdr_rosplan_interface - mas_execution_manager - mdr_move_base_action mdr_move_arm_action + mdr_move_base_action + mdr_perceive_plane_action mdr_pickup_action mdr_place_action - mdr_perceive_plane_action - - roslaunch + mas_execution_manager + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/scripts/tests/c069_scenario_initialiser b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/scripts/tests/c069_scenario_initialiser index 446a4ed44..a409d372d 100755 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/scripts/tests/c069_scenario_initialiser +++ b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/scripts/tests/c069_scenario_initialiser @@ -6,7 +6,7 @@ import rosplan_knowledge_msgs.srv as rosplan_srvs import diagnostic_msgs.msg as diag_msgs from mongodb_store.message_store import MessageStoreProxy -from mcr_perception_msgs.msg import Object, Plane +from mas_perception_msgs.msg import Object, Plane class C069ScenarioInitialiser(object): def __init__(self, objects=list(), obj_names=list()): diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/move_base.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/move_base.py deleted file mode 100644 index 3afda7b8a..000000000 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/move_base.py +++ /dev/null @@ -1,109 +0,0 @@ -import time -import rospy -import actionlib -from std_msgs.msg import String -from geometry_msgs.msg import PoseStamped - -import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import rosplan_knowledge_msgs.srv as rosplan_srvs -import diagnostic_msgs.msg as diag_msgs - -from mdr_move_base_action.msg import MoveBaseAction, MoveBaseGoal -from mas_execution_manager.scenario_state_base import ScenarioStateBase - -class MoveBase(ScenarioStateBase): - def __init__(self, save_sm_state=False, **kwargs): - ScenarioStateBase.__init__(self, 'move_base', - save_sm_state=save_sm_state, - outcomes=['succeeded', 'failed', 'failed_after_retrying']) - self.sm_id = kwargs.get('sm_id', 'mdr_demo_throw_table_objects') - self.state_name = kwargs.get('state_name', 'move_base') - self.move_base_server = kwargs.get('move_base_server', 'move_base_server') - self.destination_locations = list(kwargs.get('destination_locations', list())) - self.timeout = kwargs.get('timeout', 120.) - - self.number_of_retries = kwargs.get('number_of_retries', 0) - self.retry_count = 0 - - self.move_base_client = actionlib.SimpleActionClient(self.move_base_server, - MoveBaseAction) - self.move_base_client.wait_for_server() - - def execute(self, userdata): - if self.save_sm_state: - self.save_current_state() - - # if the current location of the robot was not specified - # in the request, we query this information from the knowledge base - original_location = '' - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = 'robot_at' - result = self.attribute_fetching_client(request) - for item in result.attributes: - if not item.is_negative: - for param in item.values: - if param.key == 'bot' and param.value != self.robot_name: - break - - if param.key == 'wp': - original_location = param.value - break - break - - for destination_location in self.destination_locations: - dispatch_msg = self.get_dispatch_msg(original_location, - destination_location) - - rospy.loginfo('Sending the base to %s' % destination_location) - self.say('Going to ' + destination_location) - self.action_dispatch_pub.publish(dispatch_msg) - - self.executing = True - self.succeeded = False - start_time = time.time() - duration = 0. - while self.executing and duration < self.timeout: - rospy.sleep(0.1) - duration = time.time() - start_time - - if self.succeeded: - rospy.loginfo('Successfully reached %s' % destination_location) - original_location = destination_location - else: - rospy.logerr('Could not reach %s' % destination_location) - self.say('Could not reach ' + destination_location) - if self.retry_count == self.number_of_retries: - self.say('Aborting operation') - return 'failed_after_retrying' - self.retry_count += 1 - return 'failed' - return 'succeeded' - - def get_table_pose(self): - try: - pose = self.msg_store_client.query_named('table_pose', PoseStamped._type)[0] - return pose - except: - rospy.logerr('Error retriving knowledge about table_pose') - return None - - def get_dispatch_msg(self, original_location, destination_location): - dispatch_msg = plan_dispatch_msgs.ActionDispatch() - dispatch_msg.name = self.action_name - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'from' - arg_msg.value = original_location - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'to' - arg_msg.value = destination_location - dispatch_msg.parameters.append(arg_msg) - - return dispatch_msg diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/perceive_planes.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/perceive_planes.py deleted file mode 100644 index 0bc781fd0..000000000 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/perceive_planes.py +++ /dev/null @@ -1,67 +0,0 @@ -import time -import rospy - -import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import diagnostic_msgs.msg as diag_msgs - -from mas_execution_manager.scenario_state_base import ScenarioStateBase - -class PerceivePlanes(ScenarioStateBase): - def __init__(self, save_sm_state=False, **kwargs): - ScenarioStateBase.__init__(self, 'perceive_plane', - save_sm_state=save_sm_state, - outcomes=['succeeded', 'failed', 'failed_after_retrying']) - self.sm_id = kwargs.get('sm_id', 'mdr_demo_throw_table_objects') - self.state_name = kwargs.get('state_name', 'perceive_planes') - self.timeout = kwargs.get('timeout', 120.) - self.plane_prefix = kwargs.get('plane_prefix', 0) - - self.number_of_retries = kwargs.get('number_of_retries', 0) - self.retry_count = 0 - - def execute(self, userdata): - if self.save_sm_state: - self.save_current_state() - - dispatch_msg = self.get_dispatch_msg(self.plane_prefix) - rospy.loginfo('Perceiving plane %s' % self.plane_prefix) - self.say('Perceiving plane ' + self.plane_prefix) - self.action_dispatch_pub.publish(dispatch_msg) - - self.executing = True - self.succeeded = False - start_time = time.time() - duration = 0. - while self.executing and duration < self.timeout: - rospy.sleep(0.1) - duration = time.time() - start_time - - if self.succeeded: - rospy.loginfo('%s perceived successfully' % self.plane_prefix) - return 'succeeded' - - rospy.loginfo('Could not perceive %s' % self.plane_prefix) - self.say('Could not perceive ' + self.plane_prefix) - if self.retry_count == self.number_of_retries: - rospy.loginfo('Failed to perceive %s' % self.plane_prefix) - self.say('Aborting operation') - return 'failed_after_retrying' - rospy.loginfo('Retrying to perceive %s' % self.plane_prefix) - self.retry_count += 1 - return 'failed' - - def get_dispatch_msg(self, plane_name): - dispatch_msg = plan_dispatch_msgs.ActionDispatch() - dispatch_msg.name = self.action_name - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'plane' - arg_msg.value = plane_name - dispatch_msg.parameters.append(arg_msg) - - return dispatch_msg diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/pick.py b/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/pick.py deleted file mode 100644 index 433a556d0..000000000 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_throw_table_objects/ros/src/mdr_demo_throw_table_objects/scenario_states/pick.py +++ /dev/null @@ -1,167 +0,0 @@ -import time -import numpy as np -import rospy -from std_msgs.msg import String -from tf import TransformListener - -from geometry_msgs.msg import PointStamped - -import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import rosplan_knowledge_msgs.srv as rosplan_srvs -import diagnostic_msgs.msg as diag_msgs - -from mcr_perception_msgs.msg import Object -from mas_execution_manager.scenario_state_base import ScenarioStateBase - -class Pick(ScenarioStateBase): - def __init__(self, save_sm_state=False, **kwargs): - ScenarioStateBase.__init__(self, 'pickup', - save_sm_state=save_sm_state, - output_keys=['grasped_object'], - outcomes=['succeeded', 'failed', - 'failed_after_retrying', - 'find_objects_before_picking']) - self.sm_id = kwargs.get('sm_id', 'mdr_demo_throw_table_objects') - self.state_name = kwargs.get('state_name', 'pick') - self.timeout = kwargs.get('timeout', 120.) - self.tf_listener = TransformListener() - - self.number_of_retries = kwargs.get('number_of_retries', 0) - self.retry_count = 0 - - def execute(self, userdata): - if self.save_sm_state: - self.save_current_state() - - surface_objects = self.get_surface_objects(surface_prefix='table') - object_poses = self.get_object_poses(surface_objects) - surface, obj_to_grasp_idx = self.select_object_for_grasping(object_poses) - obj_to_grasp = '' - if obj_to_grasp_idx != -1: - obj_to_grasp = surface_objects[surface][obj_to_grasp_idx] - else: - rospy.logerr('Could not find an object to grasp') - self.say('Could not find an object to grasp') - return 'find_objects_before_picking' - - dispatch_msg = self.get_dispatch_msg(obj_to_grasp, surface) - rospy.loginfo('Picking %s from %s' % (obj_to_grasp, surface)) - self.say('Picking ' + obj_to_grasp + ' from ' + surface) - self.action_dispatch_pub.publish(dispatch_msg) - - self.executing = True - self.succeeded = False - start_time = time.time() - duration = 0. - while self.executing and duration < self.timeout: - rospy.sleep(0.1) - duration = time.time() - start_time - - if self.succeeded: - rospy.loginfo('%s grasped successfully' % obj_to_grasp) - self.say('Successfully grasped ' + obj_to_grasp) - userdata.grasped_object = obj_to_grasp - return 'succeeded' - - rospy.loginfo('Could not grasp %s' % obj_to_grasp) - self.say('Could not grasp ' + obj_to_grasp) - if self.retry_count == self.number_of_retries: - rospy.loginfo('Failed to grasp %s' % obj_to_grasp) - self.say('Aborting operation') - return 'failed_after_retrying' - rospy.loginfo('Retrying to grasp %s' % obj_to_grasp) - self.retry_count += 1 - return 'failed' - - def get_surface_objects(self, surface_prefix='table'): - surface_objects = dict() - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = 'on' - result = self.attribute_fetching_client(request) - for item in result.attributes: - object_on_desired_surface = False - object_name = '' - surface_name = '' - if not item.is_negative: - for param in item.values: - if param.key == 'plane' and param.value.find(surface_prefix) != -1: - object_on_desired_surface = True - surface_name = param.value - if surface_name not in surface_objects: - surface_objects[surface_name] = list() - elif param.key == 'obj': - object_name = param.value - if object_on_desired_surface: - surface_objects[surface_name].append(object_name) - return surface_objects - - def get_object_poses(self, surface_objects): - object_poses = dict() - for surface, objects in surface_objects.items(): - object_poses[surface] = list() - for obj_name in objects: - try: - obj = self.msg_store_client.query_named(obj_name, Object._type)[0] - object_poses[surface].append(obj.pose) - except: - rospy.logerr('Error retriving knowledge about %s', obj_name) - pass - return object_poses - - def select_object_for_grasping(self, object_poses): - '''Returns the index of the object whose distance is closest to the robot - ''' - object_surfaces = list() - distances = dict() - robot_position = np.zeros(3) - for surface, poses in object_poses.items(): - object_surfaces.append(surface) - distances[surface] = list() - for pose in poses: - base_link_pose = self.tf_listener.transformPose('base_link', pose) - distances[surface].append(self.distance(robot_position, np.array([base_link_pose.pose.position.x, - base_link_pose.pose.position.y, - base_link_pose.pose.position.z]))) - - min_dist = 1e100 - min_dist_obj_idx = -1 - obj_surface = '' - for surface, distance_list in distances.items(): - if distance_list: - surface_min_dist = np.min(distance_list) - if surface_min_dist < min_dist: - min_dist = surface_min_dist - min_dist_obj_idx = np.argmin(distance_list) - obj_surface = surface - return obj_surface, min_dist_obj_idx - - def get_robot_pose(self, map_frame='map', base_link_frame='base_link'): - latest_tf_time = self.tf_listener.getLatestCommonTime(base_link_frame, map_frame) - position, quat_orientation = self.tf_listener.lookupTransform(base_link_frame, - map_frame, - latest_tf_time) - return position, quat_orientation - - def distance(self, point1, point2): - return np.linalg.norm(np.array(point1) - np.array(point2)) - - def get_dispatch_msg(self, obj_name, surface_name): - dispatch_msg = plan_dispatch_msgs.ActionDispatch() - dispatch_msg.name = self.action_name - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'obj' - arg_msg.value = obj_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'plane' - arg_msg.value = surface_name - dispatch_msg.parameters.append(arg_msg) - - return dispatch_msg diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/CMakeLists.txt b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/CMakeLists.txt index a5e580cdb..88da30df2 100644 --- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/CMakeLists.txt +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/CMakeLists.txt @@ -8,7 +8,7 @@ find_package(catkin REQUIRED COMPONENTS smach actionlib std_msgs - mcr_door_status + mdr_door_status mdr_enter_door_action mdr_move_base_action mas_execution_manager @@ -22,7 +22,7 @@ catkin_package( actionlib std_msgs zbar_ros - mcr_door_status + mdr_door_status mdr_move_base_action mdr_enter_door_action mas_execution_manager diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/package.xml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/package.xml index f4c64f841..e62cba063 100644 --- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/package.xml +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/package.xml @@ -18,7 +18,7 @@ smach_ros actionlib std_msgs - mcr_door_status + mdr_door_status mdr_enter_door_action mdr_move_base_action mas_execution_manager @@ -29,7 +29,7 @@ actionlib std_msgs zbar_ros - mcr_door_status + mdr_door_status mdr_enter_door_action mdr_move_base_action mas_execution_manager diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/ros/launch/robot_inspection.launch b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/ros/launch/robot_inspection.launch index 0a3d824a4..344b48f62 100644 --- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/ros/launch/robot_inspection.launch +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_robot_inspection/ros/launch/robot_inspection.launch @@ -5,7 +5,7 @@ - + diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/CMakeLists.txt b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/CMakeLists.txt new file mode 100644 index 000000000..2c35cc4af --- /dev/null +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mdr_serve_drinks) + +find_package(catkin REQUIRED COMPONENTS + rospy + roslint + mas_execution_manager + mdr_move_base_action + mdr_move_arm_action + mdr_pickup_action + mdr_perceive_plane_action + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours +) + +catkin_package( + CATKIN_DEPENDS + rospy + mas_execution_manager + mdr_move_base_action + mdr_move_arm_action + mdr_pickup_action + mdr_perceive_plane_action + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours +) + +catkin_python_setup() +roslint_python() + +install(DIRECTORY ros/launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ros/launch +) diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/README.md b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/README.md new file mode 100644 index 000000000..b8934e692 --- /dev/null +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/README.md @@ -0,0 +1,47 @@ +# mdr_serve_drinks + +Your favorite drinks delivered cold and fresh. + +## Goal + +The robot has to find all party guest who don't have a drink yet and take their orders. After that the robot goes to the bar, takes the ordered drinks and brings them to the guests. This task focuses mainly on perception and human-robot interaction. +- The robot has to remember people and orders (people change places) +- One of the ordered drinks is missing + - Bonus points for preemptively informing the guest, that the order is not available +- Instead of grasping and delivering the drinks one by one the robot can optionally use an attached tray + - The barkeeper places the drinks on the tray + - The robot must detect if guests take drinks from the tray they didn't order + +## Subtasks involved + +- Perception + - Recognize objects (drinks) on a table (bar) + - Recognize people + - Recognize if people have drinks +- Speech + - Having a small conversation / dialogue +- Manipulation + - Grasping objects + - Handing over objects + +## Running the task + +1. To run rviz on your computer, export the robot's `ROS_MASTER_URI` first: + ``` + export ROS_MASTER_URI=http://:11311/ + rviz + ``` + +2. Load the config file for the robot. + +3. Use "2D Pose Estimate" to localize the robot on the map. + +4. Run the SSD Keras Image detector (make sure to have the `ROS_MASTER_URI` exported if you use a new terminal): + ``` + roslaunch ssd_keras_ros coco_image_detection_server.launch cloud_topic:=/point_cloud_topic_name + ``` + +5. Launch the serving drinks scenario (remember `ROS_MASTER_URI`) + ``` + roslaunch mdr_serve_drinks serve_drinks.launch + ``` diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/config/serve_drinks_sm.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/config/serve_drinks_sm.yaml new file mode 100644 index 000000000..798313e46 --- /dev/null +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/config/serve_drinks_sm.yaml @@ -0,0 +1,311 @@ +# Description: Defines a state machine for the RoboCup@Home "serving drinks" scenario +# Author: Henrik Schnor +# Email: henrik.schnor@smail.inf.h-brs.de +sm_id: mdr_serve_drinks +states: [GO_NEAR_BAR, FIND_BAR, GO_TO_BAR, PERCEIVE_BAR, GO_TO_LIVING_ROOM, FIND_PEOPLE, CHOOSE_ACTION, GET_ORDER_GO_TO_PERSON, GET_ORDER_DIALOGUE, GET_DRINK_GO_TO_BAR, GET_DRINK_PERCEIVE_BAR, GET_DRINK_PICK, DELIVER_DRINK_GO_TO_PERSON, DELIVER_DRINK_HAND_OVER] +outcomes: [DONE, FAILED] +state_descriptions: + # + # This is for initial observation of all available drinks + # + - state: + name: GO_NEAR_BAR + state_module_name: mdr_navigation_behaviours.move_base + state_class_name: MoveBase + transitions: + - transition: + name: succeeded + state: FIND_BAR + - transition: + name: failed + state: GO_NEAR_BAR + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: destination_locations + value: [NEAR_BAR] + - argument: + name: number_of_retries + value: 3 + - state: + name: FIND_BAR + state_module_name: mdr_perception_behaviours.find_table + state_class_name: FindTable + transitions: + - transition: + name: succeeded + state: GO_TO_BAR + - transition: + name: failed + state: GO_NEAR_BAR + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: number_of_retries + value: 3 + - state: + name: GO_TO_BAR + state_module_name: mdr_navigation_behaviours.move_base + state_class_name: MoveBase + transitions: + - transition: + name: succeeded + state: PERCEIVE_BAR + - transition: + name: failed + state: GO_NEAR_BAR + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: destination_locations + value: [BAR] + - argument: + name: number_of_retries + value: 3 + - state: + name: PERCEIVE_BAR + state_module_name: mdr_perception_behaviours.perceive_planes + state_class_name: PerceivePlanes + transitions: + - transition: + name: succeeded + state: GO_TO_LIVING_ROOM + - transition: + name: failed + state: GO_NEAR_BAR + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: plane_prefix + value: bar + - argument: + name: number_of_retries + value: 3 + + # + # Go to the living room, see who's there and decide what to do + # + - state: + name: GO_TO_LIVING_ROOM + state_module_name: mdr_navigation_behaviours.move_base + state_class_name: MoveBase + transitions: + - transition: + name: succeeded + state: FIND_PEOPLE + - transition: + name: failed + state: GO_TO_LIVING_ROOM + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: destination_locations + value: [LIVING_ROOM] + - argument: + name: number_of_retries + value: 3 + - state: + name: FIND_PEOPLE + state_module_name: mdr_perception_behaviours.find_people + state_class_name: FindPeople + transitions: + - transition: + name: succeeded + state: CHOOSE_ACTION + - transition: + name: failed + state: GO_TO_LIVING_ROOM + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: number_of_retries + value: 3 + - state: + name: CHOOSE_ACTION + state_module_name: mdr_serve_drinks.scenario_states.choose_action + state_class_name: ChooseAction + transitions: + - transition: + name: get_order + state: GET_ORDER_GO_TO_PERSON + - transition: + name: deliver_drink + state: DELIVER_DRINK_GO_TO_PERSON + - transition: + name: done + state: DONE + - transition: + name: failed + state: GO_TO_LIVING_ROOM + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: number_of_retries + value: 3 + + # + # Approach a person and ask for their order + # + - state: + name: GET_ORDER_GO_TO_PERSON + state_module_name: mdr_navigation_behaviours.move_base + state_class_name: MoveBase + transitions: + - transition: + name: succeeded + state: GET_ORDER_DIALOGUE + - transition: + name: failed + state: GO_TO_LIVING_ROOM + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: destination_locations + value: [PERSON_WITHOUT_DRINK] + - argument: + name: number_of_retries + value: 3 + - state: + name: GET_ORDER_DIALOGUE + state_module_name: mdr_speech_behaviours.dialogue + state_class_name: Dialogue + transitions: + - transition: + name: succeeded + state: GET_DRINK_GO_TO_BAR + - transition: + name: failed + state: GO_TO_LIVING_ROOM + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: dialogue_model + value: get_drink_order + + # + # Get a drink from the bar (we'll try to pick it ourselves for now) + # + - state: + name: GET_DRINK_GO_TO_BAR + state_module_name: mdr_navigation_behaviours.move_base + state_class_name: MoveBase + transitions: + - transition: + name: succeeded + state: GET_DRINK_PERCEIVE_BAR + - transition: + name: failed + state: GET_DRINK_GO_TO_BAR + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: destination_locations + value: [BAR] + - argument: + name: number_of_retries + value: 3 + - state: + name: GET_DRINK_PERCEIVE_BAR + state_module_name: mdr_perception_behaviours.perceive_planes + state_class_name: PerceivePlanes + transitions: + - transition: + name: succeeded + state: GET_DRINK_PICK + - transition: + name: failed + state: GET_DRINK_GO_TO_BAR + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: plane_prefix + value: bar + - argument: + name: number_of_retries + value: 3 + - state: + name: GET_DRINK_PICK + state_module_name: mdr_manipulation_behaviours.pick_object + state_class_name: PickObject + transitions: + - transition: + name: succeeded + state: GO_TO_LIVING_ROOM + - transition: + name: failed + state: GET_DRINK_GO_TO_BAR + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: picking_surface_prefix + value: bar + - argument: + name: number_of_retries + value: 3 + + # + # Deliver the drink to the person who ordered it + # + - state: + name: DELIVER_DRINK_GO_TO_PERSON + state_module_name: mdr_navigation_behaviours.move_base + state_class_name: MoveBase + transitions: + - transition: + name: succeeded + state: DELIVER_DRINK_HAND_OVER + - transition: + name: failed + state: DELIVER_DRINK_GO_TO_PERSON + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: destination_locations + value: [PERSON_WITH_ORDER] + - argument: + name: number_of_retries + value: 3 + - state: + name: DELIVER_DRINK_HAND_OVER + state_module_name: mdr_manipulation_behaviours.hand_over_object + state_class_name: HandOverObject + transitions: + - transition: + name: succeeded + state: GO_TO_LIVING_ROOM + - transition: + name: failed + state: DELIVER_DRINK_GO_TO_PERSON + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: number_of_retries + value: 3 diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/package.xml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/package.xml new file mode 100644 index 000000000..ba3b94926 --- /dev/null +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/package.xml @@ -0,0 +1,33 @@ + + + mdr_serve_drinks + 1.0.0 + State machine and launch files for the RoboCup@Home serving drinks scenario + GPLv3 + + Henrik Schnor + MAS robotics + + catkin + + rospy + roslint + mas_execution_manager + mdr_move_base_action + mdr_move_arm_action + mdr_pickup_action + mdr_perceive_plane_action + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours + + rospy + mas_execution_manager + mdr_move_base_action + mdr_move_arm_action + mdr_pickup_action + mdr_perceive_plane_action + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours + diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/ros/launch/serve_drinks.launch b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/ros/launch/serve_drinks.launch new file mode 100644 index 000000000..063fe05ed --- /dev/null +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/ros/launch/serve_drinks.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/mdr_speech/mdr_question_matching/ros/src/mdr_question_matching/__init__.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/ros/src/mdr_serve_drinks/__init__.py similarity index 100% rename from mdr_speech/mdr_question_matching/ros/src/mdr_question_matching/__init__.py rename to mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/ros/src/mdr_serve_drinks/__init__.py diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/ros/src/mdr_serve_drinks/scenario_states/__init__.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/ros/src/mdr_serve_drinks/scenario_states/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/setup.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/setup.py similarity index 61% rename from mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/setup.py rename to mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/setup.py index 6aed34489..d613dfe0d 100644 --- a/mdr_planning/mdr_scenarios/mdr_demos/mdr_demo_patrol/setup.py +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_serve_drinks/setup.py @@ -4,8 +4,8 @@ from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( - packages=['mdr_demo_patrol'], - package_dir={'mdr_demo_patrol': 'ros/src/mdr_demo_patrol'} + packages=['mdr_serve_drinks'], + package_dir={'mdr_serve_drinks': 'ros/src/mdr_serve_drinks'} ) setup(**d) diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_speech_person_recognition/ros/launch/spr.launch b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_speech_person_recognition/ros/launch/spr.launch index 3b321c59c..c9064fc6f 100644 --- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_speech_person_recognition/ros/launch/spr.launch +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_speech_person_recognition/ros/launch/spr.launch @@ -10,7 +10,7 @@ - + diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/CMakeLists.txt b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/CMakeLists.txt index e3a48caae..97198a92d 100644 --- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/CMakeLists.txt +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/CMakeLists.txt @@ -2,13 +2,8 @@ cmake_minimum_required(VERSION 2.8.3) project(mdr_store_groceries) find_package(catkin REQUIRED COMPONENTS - roslint rospy - smach_ros - smach - actionlib - mongodb_store - mdr_rosplan_interface + roslint mas_execution_manager mdr_listen_action mdr_process_speech_command_action @@ -17,17 +12,14 @@ find_package(catkin REQUIRED COMPONENTS mdr_pickup_action mdr_place_action mdr_perceive_plane_action + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours ) catkin_package( CATKIN_DEPENDS - roslint rospy - smach_ros - smach - actionlib - mongodb_store - mdr_rosplan_interface mas_execution_manager mdr_listen_action mdr_process_speech_command_action @@ -36,6 +28,9 @@ catkin_package( mdr_pickup_action mdr_place_action mdr_perceive_plane_action + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours ) catkin_python_setup() diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/README.md b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/README.md new file mode 100644 index 000000000..c5df15839 --- /dev/null +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/README.md @@ -0,0 +1,41 @@ +# mdr_store_groceries + +## Goal + +In this task, the robot has to correctly identify and manipulate objects at different heights, grouping them by category and likelihood. This test focuses on the detection and recognition of objects and their features, as well as object +manipulation. + +## Subtasks involved + +- Open door (not yet implemented) +- Listen for speech command to start task (Currently disabled. Note: this is not a requirement for the task and it is only used for demonstration purposes.) +- Inspect cupboard and table +- Recognize and classify objects +- Grasp objects +- Place objects + +## Running the task + +We provide and example of what needs to be run for the store groceries task. The first step is localizing the robot. You can localize it using rviz. This localization step can be done from running rviz from your computer. + +1. To run rviz in your computer, first export the ROS master from the robot. On your terminal, write: + + ``` + export ROS_MASTER_URI=http://:11311/ + rviz + ``` + +2. Load the config file with the robot model. + +3. Use the 2D Pose Estimate to localize the robot in the map. + +4. Now that the robot is localized, you can run the store groceries task. In one terminal, you need to run the SSD Keras Image detector: + ``` + roslaunch ssd_keras_ros coco_image_detection_server.launch cloud_topic:=/point_cloud_topic_name + ``` + + Then, in a different terminal: + + ``` + roslaunch mdr_store_groceries store_groceries.launch + ``` diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/config/store_groceries_sm.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/config/store_groceries_sm.yaml index 853390bb5..95f8aa53f 100644 --- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/config/store_groceries_sm.yaml +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/config/store_groceries_sm.yaml @@ -2,7 +2,7 @@ # Author: Alex Mitrevski # Email: aleksandar.mitrevski@h-brs.de sm_id: mdr_store_groceries -states: [LISTEN, PROCESS_SPEECH_COMMAND, ENTER, GO_TO_SCANNING_POSE, FIND_TABLE, GO_TO_CUPBOARD, OPEN_DOOR, SCAN_CUPBOARD, GO_TO_TABLE, PERCEIVE_TABLE, PICK_OBJECT, FIND_OBJECTS_BEFORE_PICKING, GO_BACK_TO_CUPBOARD, PLACE_OBJECT, GO_BACK_TO_TABLE, EXIT] +states: [LISTEN, PROCESS_SPEECH_COMMAND, ENTER, GO_TO_SCANNING_POSE, FIND_TABLE, GO_TO_CUPBOARD, OPEN_DOOR, SCAN_CUPBOARD, GO_TO_TABLE, PERCEIVE_TABLE, PICK_OBJECT, FIND_OBJECTS_BEFORE_PICKING, GO_BACK_TO_CUPBOARD, PLACE_OBJECT, CHECK_EMPTY_TABLE, GO_BACK_TO_TABLE, EXIT] outcomes: [DONE, TIMEOUT] state_descriptions: - state: @@ -30,7 +30,7 @@ state_descriptions: value: store groceries - state: name: ENTER - state_module_name: mdr_store_groceries.scenario_states.move_base + state_module_name: mdr_navigation_behaviours.move_base state_class_name: MoveBase transitions: - transition: @@ -51,7 +51,7 @@ state_descriptions: value: 3 - state: name: GO_TO_SCANNING_POSE - state_module_name: mdr_store_groceries.scenario_states.move_base + state_module_name: mdr_navigation_behaviours.move_base state_class_name: MoveBase transitions: - transition: @@ -93,7 +93,7 @@ state_descriptions: value: 3 - state: name: GO_TO_CUPBOARD - state_module_name: mdr_store_groceries.scenario_states.move_base + state_module_name: mdr_navigation_behaviours.move_base state_class_name: MoveBase transitions: - transition: @@ -150,7 +150,7 @@ state_descriptions: value: 3 - state: name: GO_TO_TABLE - state_module_name: mdr_store_groceries.scenario_states.move_base + state_module_name: mdr_navigation_behaviours.move_base state_class_name: MoveBase transitions: - transition: @@ -171,7 +171,7 @@ state_descriptions: value: 3 - state: name: PERCEIVE_TABLE - state_module_name: mdr_store_groceries.scenario_states.perceive_planes + state_module_name: mdr_perception_behaviours.perceive_planes state_class_name: PerceivePlanes transitions: - transition: @@ -192,8 +192,8 @@ state_descriptions: value: table - state: name: PICK_OBJECT - state_module_name: mdr_store_groceries.scenario_states.pick - state_class_name: Pick + state_module_name: mdr_manipulation_behaviours.pick_closest_from_surface + state_class_name: PickClosestFromSurface transitions: - transition: name: succeeded @@ -211,9 +211,12 @@ state_descriptions: - argument: name: number_of_retries value: 3 + - argument: + name: picking_surface_prefix + value: table - state: name: FIND_OBJECTS_BEFORE_PICKING - state_module_name: mdr_store_groceries.scenario_states.perceive_planes + state_module_name: mdr_perception_behaviours.perceive_planes state_class_name: PerceivePlanes transitions: - transition: @@ -234,7 +237,7 @@ state_descriptions: value: table - state: name: GO_BACK_TO_CUPBOARD - state_module_name: mdr_store_groceries.scenario_states.move_base + state_module_name: mdr_navigation_behaviours.move_base state_class_name: MoveBase transitions: - transition: @@ -255,15 +258,12 @@ state_descriptions: value: 3 - state: name: PLACE_OBJECT - state_module_name: mdr_store_groceries.scenario_states.place - state_class_name: Place + state_module_name: mdr_manipulation_behaviours.place_based_on_category + state_class_name: PlaceBasedOnCategory transitions: - transition: - name: pick_new_object - state: GO_BACK_TO_TABLE - - transition: - name: finished - state: EXIT + name: succeeded + state: CHECK_EMPTY_TABLE - transition: name: failed state: PLACE_OBJECT @@ -274,9 +274,27 @@ state_descriptions: - argument: name: number_of_retries value: 3 + - argument: + name: placing_surface_prefix + value: shelf + - state: + name: CHECK_EMPTY_TABLE + state_module_name: mdr_knowledge_behaviours.check_empty_surface + state_class_name: CheckEmptySurface + transitions: + - transition: + name: empty + state: EXIT + - transition: + name: not_empty + state: GO_BACK_TO_TABLE + arguments: + - argument: + name: surface_prefix + value: table - state: name: GO_BACK_TO_TABLE - state_module_name: mdr_store_groceries.scenario_states.move_base + state_module_name: mdr_navigation_behaviours.move_base state_class_name: MoveBase transitions: - transition: @@ -297,7 +315,7 @@ state_descriptions: value: 3 - state: name: EXIT - state_module_name: mdr_store_groceries.scenario_states.move_base + state_module_name: mdr_navigation_behaviours.move_base state_class_name: MoveBase transitions: - transition: diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/package.xml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/package.xml index 43c0bc593..eb7b4221d 100644 --- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/package.xml +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/package.xml @@ -2,7 +2,7 @@ mdr_store_groceries 1.0.0 - State machine and launch files for a store groceries scenario + State machine and launch files for a RoboCup@Home store groceries scenario GPLv3 Alex Mitrevski @@ -12,12 +12,6 @@ rospy roslint - smach - smach_ros - actionlib - mongodb_store - - mdr_rosplan_interface mas_execution_manager mdr_listen_action mdr_process_speech_command_action @@ -26,18 +20,11 @@ mdr_pickup_action mdr_place_action mdr_perceive_plane_action + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours rospy - roslint - smach - smach_ros - actionlib - - mongodb_store - rosplan_knowledge_base - rosplan_planning_system - - mdr_rosplan_interface mas_execution_manager mdr_listen_action mdr_process_speech_command_action @@ -46,6 +33,7 @@ mdr_pickup_action mdr_place_action mdr_perceive_plane_action - - roslaunch + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/ros/src/mdr_store_groceries/scenario_states/move_base.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/ros/src/mdr_store_groceries/scenario_states/move_base.py deleted file mode 100644 index 69656a4d2..000000000 --- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/ros/src/mdr_store_groceries/scenario_states/move_base.py +++ /dev/null @@ -1,127 +0,0 @@ -import time -import rospy -import actionlib -from std_msgs.msg import String -from geometry_msgs.msg import PoseStamped - -import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import rosplan_knowledge_msgs.srv as rosplan_srvs -import diagnostic_msgs.msg as diag_msgs - -from mdr_move_base_action.msg import MoveBaseAction, MoveBaseGoal -from mas_execution_manager.scenario_state_base import ScenarioStateBase - -class MoveBase(ScenarioStateBase): - def __init__(self, save_sm_state=False, **kwargs): - ScenarioStateBase.__init__(self, 'move_base', - save_sm_state=save_sm_state, - outcomes=['succeeded', 'failed', 'failed_after_retrying']) - self.sm_id = kwargs.get('sm_id', 'mdr_store_groceries') - self.state_name = kwargs.get('state_name', 'move_base') - self.move_base_server = kwargs.get('move_base_server', 'move_base_server') - self.destination_locations = list(kwargs.get('destination_locations', list())) - self.timeout = kwargs.get('timeout', 120.) - - self.number_of_retries = kwargs.get('number_of_retries', 0) - self.retry_count = 0 - - self.move_base_client = actionlib.SimpleActionClient(self.move_base_server, - MoveBaseAction) - self.move_base_client.wait_for_server() - - def execute(self, userdata): - if self.save_sm_state: - self.save_current_state() - - # if the current location of the robot was not specified - # in the request, we query this information from the knowledge base - original_location = '' - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = 'robot_at' - result = self.attribute_fetching_client(request) - for item in result.attributes: - if not item.is_negative: - for param in item.values: - if param.key == 'bot' and param.value != self.robot_name: - break - - if param.key == 'wp': - original_location = param.value - break - break - - for destination_location in self.destination_locations: - if destination_location.lower().find('table') != -1: - self.say('Moving to table') - - table_pose = self.get_table_pose() - if table_pose is None: - rospy.loginfo('Could not get table pose') - self.say('Could not move to table') - return 'failed' - - move_base_goal = MoveBaseGoal() - move_base_goal.goal_type = MoveBaseGoal.POSE - move_base_goal.pose = table_pose - self.move_base_client.send_goal(move_base_goal) - self.move_base_client.wait_for_result() - result = self.move_base_client.get_result() - if result: - return 'succeeded' - else: - dispatch_msg = self.get_dispatch_msg(original_location, - destination_location) - - rospy.loginfo('Sending the base to %s' % destination_location) - self.say('Going to ' + destination_location) - self.action_dispatch_pub.publish(dispatch_msg) - - self.executing = True - self.succeeded = False - start_time = time.time() - duration = 0. - while self.executing and duration < self.timeout: - rospy.sleep(0.1) - duration = time.time() - start_time - - if self.succeeded: - rospy.loginfo('Successfully reached %s' % destination_location) - original_location = destination_location - else: - rospy.logerr('Could not reach %s' % destination_location) - self.say('Could not reach ' + destination_location) - if self.retry_count == self.number_of_retries: - self.say('Aborting operation') - return 'failed_after_retrying' - self.retry_count += 1 - return 'failed' - return 'succeeded' - - def get_table_pose(self): - try: - pose = self.msg_store_client.query_named('table_pose', PoseStamped._type)[0] - return pose - except: - rospy.logerr('Error retriving knowledge about table_pose') - return None - - def get_dispatch_msg(self, original_location, destination_location): - dispatch_msg = plan_dispatch_msgs.ActionDispatch() - dispatch_msg.name = self.action_name - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'from' - arg_msg.value = original_location - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'to' - arg_msg.value = destination_location - dispatch_msg.parameters.append(arg_msg) - - return dispatch_msg diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/ros/src/mdr_store_groceries/scenario_states/pick.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/ros/src/mdr_store_groceries/scenario_states/pick.py deleted file mode 100644 index 2eb8fc701..000000000 --- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/ros/src/mdr_store_groceries/scenario_states/pick.py +++ /dev/null @@ -1,167 +0,0 @@ -import time -import numpy as np -import rospy -from std_msgs.msg import String -from tf import TransformListener - -from geometry_msgs.msg import PointStamped - -import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import rosplan_knowledge_msgs.srv as rosplan_srvs -import diagnostic_msgs.msg as diag_msgs - -from mcr_perception_msgs.msg import Object -from mas_execution_manager.scenario_state_base import ScenarioStateBase - -class Pick(ScenarioStateBase): - def __init__(self, save_sm_state=False, **kwargs): - ScenarioStateBase.__init__(self, 'pickup', - save_sm_state=save_sm_state, - output_keys=['grasped_object'], - outcomes=['succeeded', 'failed', - 'failed_after_retrying', - 'find_objects_before_picking']) - self.sm_id = kwargs.get('sm_id', 'mdr_store_groceries') - self.state_name = kwargs.get('state_name', 'pick') - self.timeout = kwargs.get('timeout', 120.) - self.tf_listener = TransformListener() - - self.number_of_retries = kwargs.get('number_of_retries', 0) - self.retry_count = 0 - - def execute(self, userdata): - if self.save_sm_state: - self.save_current_state() - - surface_objects = self.get_surface_objects(surface_prefix='table') - object_poses = self.get_object_poses(surface_objects) - surface, obj_to_grasp_idx = self.select_object_for_grasping(object_poses) - obj_to_grasp = '' - if obj_to_grasp_idx != -1: - obj_to_grasp = surface_objects[surface][obj_to_grasp_idx] - else: - rospy.logerr('Could not find an object to grasp') - self.say('Could not find an object to grasp') - return 'find_objects_before_picking' - - dispatch_msg = self.get_dispatch_msg(obj_to_grasp, surface) - rospy.loginfo('Picking %s from %s' % (obj_to_grasp, surface)) - self.say('Picking ' + obj_to_grasp + ' from ' + surface) - self.action_dispatch_pub.publish(dispatch_msg) - - self.executing = True - self.succeeded = False - start_time = time.time() - duration = 0. - while self.executing and duration < self.timeout: - rospy.sleep(0.1) - duration = time.time() - start_time - - if self.succeeded: - rospy.loginfo('%s grasped successfully' % obj_to_grasp) - self.say('Successfully grasped ' + obj_to_grasp) - userdata.grasped_object = obj_to_grasp - return 'succeeded' - - rospy.loginfo('Could not grasp %s' % obj_to_grasp) - self.say('Could not grasp ' + obj_to_grasp) - if self.retry_count == self.number_of_retries: - rospy.loginfo('Failed to grasp %s' % obj_to_grasp) - self.say('Aborting operation') - return 'failed_after_retrying' - rospy.loginfo('Retrying to grasp %s' % obj_to_grasp) - self.retry_count += 1 - return 'failed' - - def get_surface_objects(self, surface_prefix='table'): - surface_objects = dict() - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = 'on' - result = self.attribute_fetching_client(request) - for item in result.attributes: - object_on_desired_surface = False - object_name = '' - surface_name = '' - if not item.is_negative: - for param in item.values: - if param.key == 'plane' and param.value.find(surface_prefix) != -1: - object_on_desired_surface = True - surface_name = param.value - if surface_name not in surface_objects: - surface_objects[surface_name] = list() - elif param.key == 'obj': - object_name = param.value - if object_on_desired_surface: - surface_objects[surface_name].append(object_name) - return surface_objects - - def get_object_poses(self, surface_objects): - object_poses = dict() - for surface, objects in surface_objects.items(): - object_poses[surface] = list() - for obj_name in objects: - try: - obj = self.msg_store_client.query_named(obj_name, Object._type)[0] - object_poses[surface].append(obj.pose) - except: - rospy.logerr('Error retriving knowledge about %s', obj_name) - pass - return object_poses - - def select_object_for_grasping(self, object_poses): - '''Returns the index of the object whose distance is closest to the robot - ''' - object_surfaces = list() - distances = dict() - robot_position = np.zeros(3) - for surface, poses in object_poses.items(): - object_surfaces.append(surface) - distances[surface] = list() - for pose in poses: - base_link_pose = self.tf_listener.transformPose('base_link', pose) - distances[surface].append(self.distance(robot_position, np.array([base_link_pose.pose.position.x, - base_link_pose.pose.position.y, - base_link_pose.pose.position.z]))) - - min_dist = 1e100 - min_dist_obj_idx = -1 - obj_surface = '' - for surface, distance_list in distances.items(): - if distance_list: - surface_min_dist = np.min(distance_list) - if surface_min_dist < min_dist: - min_dist = surface_min_dist - min_dist_obj_idx = np.argmin(distance_list) - obj_surface = surface - return obj_surface, min_dist_obj_idx - - def get_robot_pose(self, map_frame='map', base_link_frame='base_link'): - latest_tf_time = self.tf_listener.getLatestCommonTime(base_link_frame, map_frame) - position, quat_orientation = self.tf_listener.lookupTransform(base_link_frame, - map_frame, - latest_tf_time) - return position, quat_orientation - - def distance(self, point1, point2): - return np.linalg.norm(np.array(point1) - np.array(point2)) - - def get_dispatch_msg(self, obj_name, surface_name): - dispatch_msg = plan_dispatch_msgs.ActionDispatch() - dispatch_msg.name = self.action_name - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'obj' - arg_msg.value = obj_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'plane' - arg_msg.value = surface_name - dispatch_msg.parameters.append(arg_msg) - - return dispatch_msg diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/ros/src/mdr_store_groceries/scenario_states/place.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/ros/src/mdr_store_groceries/scenario_states/place.py deleted file mode 100644 index 456811f95..000000000 --- a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_store_groceries/ros/src/mdr_store_groceries/scenario_states/place.py +++ /dev/null @@ -1,202 +0,0 @@ -import time -import numpy as np -import rospy -from std_msgs.msg import String - -import rosplan_dispatch_msgs.msg as plan_dispatch_msgs -import rosplan_knowledge_msgs.srv as rosplan_srvs -import diagnostic_msgs.msg as diag_msgs - -from mcr_perception_msgs.msg import Object -from mas_execution_manager.scenario_state_base import ScenarioStateBase - -class Place(ScenarioStateBase): - def __init__(self, save_sm_state=False, **kwargs): - ScenarioStateBase.__init__(self, 'place', - save_sm_state=save_sm_state, - input_keys=['grasped_object'], - outcomes=['pick_new_object', 'finished', - 'failed', 'failed_after_retrying']) - self.sm_id = kwargs.get('sm_id', 'mdr_store_groceries') - self.state_name = kwargs.get('state_name', 'place') - self.timeout = kwargs.get('timeout', 120.) - - self.number_of_retries = kwargs.get('number_of_retries', 0) - self.retry_count = 0 - - def execute(self, userdata): - if self.save_sm_state: - self.save_current_state() - - grasped_object = userdata.grasped_object - grasped_obj_category = self.get_object_category(grasped_object) - surface_name = self.choose_placing_surface(grasped_object, grasped_obj_category) - dispatch_msg = self.get_dispatch_msg(grasped_object, surface_name) - rospy.loginfo('Placing %s on %s' % (grasped_object, surface_name)) - self.say('Placing ' + grasped_object + ' on ' + surface_name) - self.action_dispatch_pub.publish(dispatch_msg) - - self.executing = True - self.succeeded = False - start_time = time.time() - duration = 0. - while self.executing and duration < self.timeout: - rospy.sleep(0.1) - duration = time.time() - start_time - - if self.succeeded: - rospy.loginfo('Object placed successfully') - self.say('Successfully placed ' + grasped_object) - if self.surface_empty(surface_prefix='table'): - self.say('I am finally done storing the groceries') - return 'finished' - return 'pick_new_object' - - rospy.loginfo('Could not place object %s' % grasped_object) - self.say('Could not place ' + obj_to_grasp) - if self.retry_count == self.number_of_retries: - rospy.loginfo('Failed to place object %s' % grasped_object) - self.say('Aborting operation') - return 'failed_after_retrying' - rospy.loginfo('Retrying to place object %s' % grasped_object) - self.retry_count += 1 - return 'failed' - - def get_object_category(self, obj_name): - try: - obj = self.msg_store_client.query_named(obj_name, Object._type)[0] - return obj.category - except: - rospy.logerr('Error retriving knowledge about %s', obj_name) - return '' - - def choose_placing_surface(self, obj_name, obj_category): - obj_category_map = self.get_obj_category_map() - surface_category_counts = self.get_surface_category_counts(obj_category_map) - grasped_object_category = obj_category_map[obj_name] - placing_surface = self.get_best_placing_surface(obj_category, - surface_category_counts) - return placing_surface - - def get_obj_category_map(self): - '''Returns a dictionary of objects and object categories in which - each key represents an object and the value is its category - ''' - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = 'object_category' - category_result = self.attribute_fetching_client(request) - - obj_category_dict = dict() - for item in category_result.attributes: - obj_name = '' - obj_category = '' - for param in item.values: - if param.key == 'obj': - obj_name = param.value - elif param.key == 'cat': - obj_category = param.value - obj_category_dict[obj_name] = obj_category - return obj_category_dict - - def get_surface_category_counts(self, obj_category_map): - '''Returns a dictionary of surfaces and object category counts in which - each key represents a surface in the environment and the value - is a dictionary of object counts for each category - ''' - surface_category_counts = dict() - - # we take all explored surfaces and populate 'surface_category_counts' - # with an empty dictionary for each surface; each such dictionary - # will store the object category count for the respective surface - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = 'explored' - explored_result = self.attribute_fetching_client(request) - for item in explored_result.attributes: - surface_name = '' - for param in item.values: - if param.key == 'plane': - surface_name = param.value - # we don't want to place items on the table, so we - # don't consider the table as a placing surface - if surface_name not in surface_category_counts and surface_name.find('table') == -1: - surface_category_counts[surface_name] = dict() - - # we collect a dictionary of object category counts for each surface - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = 'on' - on_result = self.attribute_fetching_client(request) - for item in on_result.attributes: - obj_name = '' - obj_surface = '' - for param in item.values: - if param.key == 'obj': - obj_name = param.value - elif param.key == 'plane': - obj_surface = param.value - - if obj_surface.find('table') == -1: - obj_category = obj_category_map[obj_name] - if obj_category not in surface_category_counts[obj_surface]: - surface_category_counts[obj_surface][obj_category] = 1 - else: - surface_category_counts[obj_surface][obj_category] += 1 - return surface_category_counts - - def get_best_placing_surface(self, obj_category, surface_category_counts): - '''If none of the surfaces contain objects whose category is - 'obj_category', returns the name of a random surface; otherwise, - returns the name of the surface that has the largest number - of objects of the category 'obj_category'. - ''' - surfaces = list() - obj_surface_category_counts = list() - for surface, category_counts in surface_category_counts.items(): - surfaces.append(surface) - if obj_category in category_counts: - obj_surface_category_counts.append(category_counts[obj_category]) - else: - obj_surface_category_counts.append(0) - - surface_idx = -1 - if np.count_nonzero(obj_surface_category_counts) != 0: - surface_idx = np.argmax(obj_surface_category_counts) - else: - surface_idx = np.random.randint(0, len(surfaces)) - return surfaces[surface_idx] - - def surface_empty(self, surface_prefix='table'): - no_objects_on_surface = True - request = rosplan_srvs.GetAttributeServiceRequest() - request.predicate_name = 'on' - result = self.attribute_fetching_client(request) - for item in result.attributes: - object_on_desired_surface = False - if not item.is_negative: - for param in item.values: - if param.key == 'plane' and param.value.find(surface_prefix) != -1: - object_on_desired_surface = True - if object_on_desired_surface: - no_objects_on_surface = False - break - return no_objects_on_surface - - def get_dispatch_msg(self, obj_name, plane_name): - dispatch_msg = plan_dispatch_msgs.ActionDispatch() - dispatch_msg.name = self.action_name - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'bot' - arg_msg.value = self.robot_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'obj' - arg_msg.value = obj_name - dispatch_msg.parameters.append(arg_msg) - - arg_msg = diag_msgs.KeyValue() - arg_msg.key = 'plane' - arg_msg.value = plane_name - dispatch_msg.parameters.append(arg_msg) - - return dispatch_msg diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/CMakeLists.txt b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/CMakeLists.txt new file mode 100644 index 000000000..d01a07511 --- /dev/null +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mdr_take_out_garbage) + +find_package(catkin REQUIRED COMPONENTS + rospy + roslint + mas_execution_manager + mdr_listen_action + mdr_process_speech_command_action + mdr_move_base_action + mdr_move_arm_action + mdr_pickup_action + mdr_place_action + mdr_perceive_plane_action + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours +) + +catkin_package( + CATKIN_DEPENDS + rospy + mas_execution_manager + mdr_listen_action + mdr_process_speech_command_action + mdr_move_base_action + mdr_move_arm_action + mdr_pickup_action + mdr_place_action + mdr_perceive_plane_action + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours +) + +catkin_python_setup() +roslint_python() + +install(DIRECTORY ros/launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ros/launch +) diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/README.md b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/README.md new file mode 100644 index 000000000..6dfc4c2f3 --- /dev/null +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/README.md @@ -0,0 +1,5 @@ +# mdr_take_out_garbage + +## Goal + +In this task, the robot has to out the trash bags from the two bins located inside a room. All garbage bins in the apartment are emptied by the robot and the garbage bags are subsequently moved to a specified collection zone. diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/config/take_out_garbage_sm.yaml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/config/take_out_garbage_sm.yaml new file mode 100644 index 000000000..eebfb95f7 --- /dev/null +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/config/take_out_garbage_sm.yaml @@ -0,0 +1,249 @@ +# Description: Defines a state machine for a RoboCup@Home take out garbage scenario +# Author: Roberto Cai +# Email: roberto.cai@smail.inf.h-brs.de +sm_id: mdr_take_out_garbage +states: [LISTEN, PROCESS_SPEECH_COMMAND, ENTER, GO_TO_BIN, FIND_BIN, PERCEIVE_INSIDE_BIN, PICK_OBJECT, GO_TO_COLLECTION_ZONE, PLACE_OBJECT, CHECK_BINS_LEFT, EXIT] +outcomes: [DONE, TIMEOUT] +state_descriptions: + - state: + name: LISTEN + state_module_name: mdr_listen_action.action_states + state_class_name: Listen + transitions: + - transition: + name: received_command + state: PROCESS_SPEECH_COMMAND + - state: + name: PROCESS_SPEECH_COMMAND + state_module_name: mdr_process_speech_command_action.action_states + state_class_name: ProcessCommand + transitions: + - transition: + name: start_take_out_garbage + state: ENTER + - transition: + name: continue_waiting + state: LISTEN + arguments: + - argument: + name: start_command + value: take out garbage + - state: + name: ENTER + state_module_name: mdr_navigation_behaviours.move_base + state_class_name: MoveBase + transitions: + - transition: + name: succeeded + state: GO_TO_BIN + - transition: + name: failed + state: ENTER + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: destination_locations + value: [INSIDE_ARENA] + - argument: + name: number_of_retries + value: 3 + - state: + name: GO_TO_BIN + state_module_name: mdr_navigation_behaviours.move_base + state_class_name: MoveBase + transitions: + - transition: + name: succeeded + state: FIND_BIN + - transition: + name: failed + state: GO_TO_BIN + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: destination_locations + value: [BIN] + - argument: + name: number_of_retries + value: 3 + - state: + name: FIND_BIN + state_module_name: mdr_take_out_garbage.scenario_states + state_class_name: FindBin + transitions: + - transition: + name: succeeded + state: PERCEIVE_INSIDE_BIN + - transition: + name: failed + state: GO_TO_BIN + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: destination_locations + value: [BIN] + - argument: + name: number_of_retries + value: 3 + - state: + name: PERCEIVE_INSIDE_BIN + state_module_name: mdr_perception_behaviours.perceive_inside_bin + state_class_name: PerceiveInsideBin + transitions: + - transition: + name: succeeded + state: PICK_GARBAGE_BAG + - transition: + name: failed + state: PERCEIVE_INSIDE_BIN + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: number_of_retries + value: 3 + - state: + name: PICK_GARBAGE_BAG + state_module_name: mdr_manipulation_behaviours.pick_non_rigid_object_from_bin + state_class_name: PickNonRigidObjectFromBin + transitions: + - transition: + name: succeeded + state: GO_TO_COLLECTION_ZONE + - transition: + name: failed + state: PICK_GARBAGE_BAG + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: number_of_retries + value: 3 + - state: + name: GO_TO_COLLECTION_ZONE + state_module_name: mdr_navigation_behaviours.move_base + state_class_name: MoveBase + transitions: + - transition: + name: succeeded + state: FIND_COLLECTION_ZONE + - transition: + name: failed + state: GO_TO_COLLECTION_ZONE + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: destination_locations + value: [COLLECTION_ZONE] + - argument: + name: number_of_retries + value: 3 + - state: + name: FIND_COLLECTION_ZONE + state_module_name: mdr_take_out_garbage.scenario_states + state_class_name: FindCollectionZone + transitions: + - transition: + name: succeeded + state: PERCEIVE_TABLE + - transition: + name: failed + state: GO_TO_COLLECTION_ZONE + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: number_of_retries + value: 3 + - argument: + name: placing_surface_prefix + value: collection_zone + - state: + name: PERCEIVE_COLLECTION_ZONE + state_module_name: mdr_perception_behaviours.perceive_planes + state_class_name: PerceivePlanes + transitions: + - transition: + name: succeeded + state: PLACE_OBJECT + - transition: + name: failed + state: PERCEIVE_TABLE + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: number_of_retries + value: 3 + - argument: + name: plane_prefix + value: collection_zone + - state: + name: PLACE_OBJECT + state_module_name: mdr_manipulation_behaviours.place + state_class_name: Place + transitions: + - transition: + name: succeeded + state: CHECK_BINS_LEFT + - transition: + name: failed + state: PLACE_OBJECT + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: number_of_retries + value: 3 + - argument: + name: placing_surface_prefix + value: collection_zone + - state: + name: CHECK_BINS_LEFT + state_module_name: mdr_knowledge_behaviours.check_bins_left + state_class_name: CheckBinsLeft + transitions: + - transition: + name: no_bins_left + state: EXIT + - transition: + name: bins_still_left + state: GO_TO_BIN + arguments: + - argument: + name: + value: + - state: + name: EXIT + state_module_name: mdr_navigation_behaviours.move_base + state_class_name: MoveBase + transitions: + - transition: + name: succeeded + state: DONE + - transition: + name: failed + state: EXIT + - transition: + name: failed_after_retrying + state: FAILED + arguments: + - argument: + name: destination_locations + value: [OUTSIDE_ARENA] + - argument: + name: number_of_retries + value: 3 diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/package.xml b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/package.xml new file mode 100644 index 000000000..b44b219c0 --- /dev/null +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/package.xml @@ -0,0 +1,40 @@ + + + mdr_take_out_garbage + 1.0.0 + The mdr_take_out_garbage package + GPLv3 + + Alex Mitrevski + MAS robotics + + + catkin + + rospy + roslint + mas_execution_manager + mdr_listen_action + mdr_process_speech_command_action + mdr_move_base_action + mdr_move_arm_action + mdr_pickup_action + mdr_place_action + mdr_perceive_plane_action + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours + + rospy + mas_execution_manager + mdr_listen_action + mdr_process_speech_command_action + mdr_move_base_action + mdr_move_arm_action + mdr_pickup_action + mdr_place_action + mdr_perceive_plane_action + mdr_navigation_behaviours + mdr_perception_behaviours + mdr_manipulation_behaviours + diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/ros/launch/take_out_garbage.launch b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/ros/launch/take_out_garbage.launch new file mode 100644 index 000000000..3d24b7b01 --- /dev/null +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/ros/launch/take_out_garbage.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/ros/src/mdr_take_out_garbage/__init__.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/ros/src/mdr_take_out_garbage/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/ros/src/mdr_take_out_garbage/scenario_states/__init__.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/ros/src/mdr_take_out_garbage/scenario_states/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/setup.py b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/setup.py similarity index 59% rename from mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/setup.py rename to mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/setup.py index 7eeb60638..10ea35811 100644 --- a/mdr_planning/mdr_actions/mdr_perception_actions/mdr_perceive_table/setup.py +++ b/mdr_planning/mdr_scenarios/mdr_robocup_tasks/mdr_take_out_garbage/setup.py @@ -4,8 +4,8 @@ from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( - packages=['mdr_perceive_table'], - package_dir={'mdr_perceive_table': 'ros/src/mdr_perceive_table'} + packages=['mdr_take_out_garbage'], + package_dir={'mdr_take_out_garbage': 'ros/src/mdr_take_out_garbage'} ) setup(**d) diff --git a/mdr_speech/README.md b/mdr_speech/README.md index 30a0a4d8c..de2912ceb 100644 --- a/mdr_speech/README.md +++ b/mdr_speech/README.md @@ -1,53 +1,4 @@ -# mdr_speech +mdr_speech -This folder contains the speech recognition MS windows components -- SpeechRecognition is the SAPI 5.4 based API -- ros is a RosWindows wrapper for integration of the API -- test allows to run the recognition without ROS e.g. to test grammars +This folder contains speech recognition components. -- it uses the brsu_sppech_common folder for certain string operations and the config file reader - -### Build Requirements -- Windows 7 -- Visual Studio 2008 or greater (Not express edition since COM objects are used) -- ROSWindows 1.1.0 (http://www.servicerobotics.eu/index.php?id=37) - - -### Recompile ROS messages and Services -If you change the ROS Services or Messages, you have to recompile them in Linux since ROSWin has no facilities for doing that yet. The generated files can/must be used in windows -Linux: roscd brsu_srvs -Linux: Type "rosmake" -Linux: roscd brsu_msgs -Linux: Type "rosmake" -Linux: Commit in git -Windows: Pull in git - - -### Services -ChangeGrammar.srv in brsu_common/srvs -rosservice call /changeGrammar "grammarfile" ("Grammar file will be loaded relative to windows server /SpeechRecognition/grammars folder!") - - -### Topics -Publishes "RecognizedSpeech.msg" in brsu_common/msgs -Message contains: -- String: The full speech sentence that has been understood -- String: A keyword related to the sentence for the scheduler -- Int32: Confidence of the speech recognizer (1 = high, 0 = mid, -1 = low) -- String[]: all keywords identified in the last recognized speech - -### Change or Enhance Grammars -Grammars can be tested by dragging and dropping the grammar file over the gc.bat in the grammar folder -All variables used in the programm and grammars are lower case and with underscore -Grammars can be changed accorsign to the official SAPI grammars docu: http://msdn.microsoft.com/en-us/library/ee125672(v=VS.85).aspx - -ATTENTION: -To make it possible to reference other grammar files with the rule ref tag, a custom parser was developed (SpeechRecognition/src/GrammarParser.cpp) since the microsoft parser seems not to work -This parser loads a grammer, checks all ruleref tags for the URI and combines them into one single grammar file (tempGrammar.xml). This tempGrammar will exist after the -programm is excited and can be checked with the grammar compiler if any problems occured. -This has 2 implications: -- Only 1 rule ref per line is allowed -- the URL tag will be checked for files ONLY! - -## Contributors -- Mike - **Original author** diff --git a/mdr_speech/mdr_question_matching/CHANGELOG.rst b/mdr_speech/mdr_question_matching/CHANGELOG.rst deleted file mode 100644 index c3754b2cb..000000000 --- a/mdr_speech/mdr_question_matching/CHANGELOG.rst +++ /dev/null @@ -1,37 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package mdr_question_matching -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.2.0 (2017-11-04) ------------------- - -1.1.2 (2017-10-29) ------------------- -* Add missing scripts to installs in CMakeLists.txt -* Fix lint issues on mdr_question_matcher -* Contributors: Argentina Ortega Sainz - -1.1.1 (2017-09-20) ------------------- -* Change maintainer tags to MAS Robotics -* Generate changelogs for mdr_speech -* Contributors: Argentina Ortega Sainz - -1.1.0 (2017-09-18 18:15) ------------------------- - -1.0.1 (2017-09-18 18:04) ------------------------- -* Merge branch 'refactor/mdr_speech' into 'indigo' - Refactor/mdr speech - See merge request !30 -* Add mcr_question_matching from mas_common_robotics -* Contributors: Argentina Ortega Sainz, Alex Mitrevski - -1.0.0 (2017-04-11) ------------------- -* Added QAgrammar.txt -* Q&A are loaded from a txt file -* Fixed a problem with matching answers -* Add question matching package from Iryna -* Contributors: Iryna Ivanovska, Santosh Thoduka diff --git a/mdr_speech/mdr_question_matching/CMakeLists.txt b/mdr_speech/mdr_question_matching/CMakeLists.txt deleted file mode 100644 index fe203f1ce..000000000 --- a/mdr_speech/mdr_question_matching/CMakeLists.txt +++ /dev/null @@ -1,32 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(mdr_question_matching) - -find_package(catkin REQUIRED - COMPONENTS - roslint - rospy -) - - - -catkin_python_setup() - -catkin_package( - CATKIN_DEPENDS - std_msgs -) - -roslint_python() - -### TESTS -if(CATKIN_ENABLE_TESTING) - find_package(roslaunch REQUIRED) - roslaunch_add_file_check(ros/launch) -endif() - - -### INSTALLS -install(PROGRAMS - ros/scripts/question_matcher - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/mdr_speech/mdr_question_matching/package.xml b/mdr_speech/mdr_question_matching/package.xml deleted file mode 100644 index fd3281b0e..000000000 --- a/mdr_speech/mdr_question_matching/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - mdr_question_matching - 1.2.0 - - Matches questions with answers - - - GPLv3 - - Iryna Ivanovska - - MAS robotics - - catkin - - rospy - roslint - - std_msgs - - roslaunch - - diff --git a/mdr_speech/mdr_question_matching/ros/config/go2018.txt b/mdr_speech/mdr_question_matching/ros/config/go2018.txt deleted file mode 100644 index 402571953..000000000 --- a/mdr_speech/mdr_question_matching/ros/config/go2018.txt +++ /dev/null @@ -1,22 +0,0 @@ -What is Robotics: The scientific study of robot. -Who is considered as the father of industrial robot: Joseph Engelberger. -Give one example for a computer programming language that can be used for robot programming:AML (A Manufacturing Language) -What is the major disadvantage of using a robot:Heavy investment. -Name the language used in Expert system:LISP and PROLOG. -Name one of the most important parts in Expert system:Knowledge Base. -Which system was designed for diagnosis and therapy recommendation for infectious disease:MYCIN. -What is the importance of AI:Human intelligence have certain limit in speed and accuracy. It is interrupted due to the lack of presence of mind, mood, etc. Bu AI systems have their own superb abilities. -Weaving The Web was written by:Tim Burners Lee -What is Beta Test:Trial test of a computer or software before the commercial launch -What is the extension of PDF:Portable document format -Expand RDBMS:Relational Data Base Management System -Difference engine was developed by:Charles Babbage -Orkut.com is now owned by:Google -Worlds first microprocessor is:Intel 4004 -What is SQL:Structured Query Language -What is the expansion of SMS:Short Message Service -Which IT company's nickname is The Big Blue:IBM -What is the full form of IEEE:Institute of Electric and Electronic Engineers -Email was developed by:Raymond Samuel Tomlinson Ray Tomlinson -Who is Netizen :Net Citizen Citizen who uses internet -What is Scareware:Fake antivirus softwares diff --git a/mdr_speech/mdr_question_matching/ros/config/questions.txt b/mdr_speech/mdr_question_matching/ros/config/questions.txt deleted file mode 100644 index 5b0581b92..000000000 --- a/mdr_speech/mdr_question_matching/ros/config/questions.txt +++ /dev/null @@ -1,68 +0,0 @@ -who are the inventors of the c programming language:inventors of the c programming language are ken thompson and dennis ritchie -who is the inventor of the python programming language:inventor of the python programming language is guido van rossum -which robot was the star in the movie wall-e:star in the movie wallee is wallee -which robot was the star in the movie wallee:star in the movie wallee is wallee -where does the term computer bug come from:from a moth trapped in a relay -what is the name of the round robot in the new star wars movie:round robot in the new star wars movie is b b 8 -how many curry sausages are eaten in germany each year:about 800 million currywurst every year -who is president of the galaxy in the hitchhikers guide to the galaxy:zaphod beeblebrox -who is president of the galaxy in the hitchhiker guide to the galaxy:zaphod beeblebrox -which robot is the love interest in wallee:eve -which robot is the love interest in wall-e:eve -which company makes asimo:asimo is made by honda -what company makes big dog:big dog is made by boston dynamics -what is the funny clumsy character of the star wars prequels:the funny clumsy character of the star wars prequels is jar jar binks -how many people live in the germany: a little over 80 million -what are the colours of the german flag: black red and yellow -what city is the capital of the germany:the capital of the germany is berlin -how many arms do you have:i have one arm -what is the heaviest element:plutonium when measured by the mass of the element but osmium is densest -what did alan turing create:many things like turing machines and the turing test -who is the helicopter pilot in the a-team:captain howling mad murdock -what apollo was the last to land on the moon:apollo 17 -who was the last man to step on the moon:the last man to step on the moon was gene cernan -in which county is the play of hamlet set:denmark -what are names of donald ducks nephews:huey dewey and louie duck -what are names of donald duck nephews:huey dewey and louie duck -how many metres are in a mile:about 1609 meters -name a dragon in the lord of the rings:name of the dragon in the lord of the rings is smaug -who is the chancellor of germany:angela merkel -who developed the first industrial robot:the american physicist joseph engelberg. he is also considered the father of robotics -what's the difference between a cyborg and an android:cyborgs are biological being with electromechanical enhancements. androids are human-shaped robots -what is the difference between a cyborg and an android:cyborgs are biological being with electromechanical enhancements. androids are human-shaped robots -do you know any cyborg:professor kevin warwick. he implanted a chip in in his left arm to remotely operate doors an artificial hand and an electronic wheelchair -in which city is this years robocup hosted:in leipzig germany -in which city is this year's robocup hosted:in leipzig germany -which city hosted last years robocup:hefei china -which city hosted last year's robocup:hefei china -in which city will next years robocup be hosted:nagoya in japan -in which city will next year's robocup be hosted:nagoya in japan -name the main rivers surrounding leipzig:the parthe pleisse and the white elster -where is the zoo of this city located:near the central station -where did the peaceful revolution of 1989 start:the peaceful revolution started in september 4 1989 in leipzig at the st. nicholas church -where is the world's oldest trade fair hosted:the worlds oldest trade fair is in leipzig -where is the worlds oldest trade fair hosted:the worlds oldest trade fair is in leipzig -where is one of the world's largest dark music festivals hosted:leipzig hosts one of the worlds largest dark music festivals -where is one of the worlds largest dark music festivals hosted:leipzig hosts one of the worlds largest dark music festivals -where is one of the world largest dark music festivals hosted:leipzig hosts one of the world largest dark music festivals -where is europes oldest continuous coffee shop hosted:europes oldest continuous coffee shop is in leipzig -where is europe's oldest continuous coffee shop hosted:europes oldest continuous coffee shop is in leipzig -where is europe oldest continuous coffee shop hosted:europe oldest continuous coffee shop is in leipzig -name one of the greatest german composers:johann sebastian bach -where is johann sebastian bach buried:johann sebastian bach is buried in st. thomas church here in leipzig -where is johann sebastian bach buried:johann sebastian bach is buried in sant thomas church here in leipzig -do you have dreams:i dream of electric sheeps -hey whats up:i dont know since ive never been there -hey whats up:i do not know since i have never been there -hey what up:i dont know since ive never been there -hey what's up:i dont know since ive never been there -there are seven days in a week true or false:true there are seven days in a week -there are eleven days in a week true or false:false there are seven days in a week not eleven -january has 31 days true or false:true january has 31 days -january has 28 days true or false:false january has 31 days not 28 -february has 28 days true or false:true but in leap-years has 29 -february has 31 days true or false:false february has either 28 or 29 days, depend on the year -what city are you from: i am from sankt augustin -what city are you from: im from sankt augustin -who used first the word robot:the word robot was first used by czech writer karel capek -what origin has the word robot:the czech word robota that means forced work or labour diff --git a/mdr_speech/mdr_question_matching/ros/launch/question_matching.launch b/mdr_speech/mdr_question_matching/ros/launch/question_matching.launch deleted file mode 100644 index a17a3e89d..000000000 --- a/mdr_speech/mdr_question_matching/ros/launch/question_matching.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/mdr_speech/mdr_question_matching/ros/scripts/question_matcher b/mdr_speech/mdr_question_matching/ros/scripts/question_matcher deleted file mode 100755 index 4119ec922..000000000 --- a/mdr_speech/mdr_question_matching/ros/scripts/question_matcher +++ /dev/null @@ -1,7 +0,0 @@ -#!/usr/bin/env python - -import mdr_question_matching.question_matcher - - -if __name__ == '__main__': - mdr_question_matching.question_matcher.main() diff --git a/mdr_speech/mdr_question_matching/ros/src/mdr_question_matching/question_matcher.py b/mdr_speech/mdr_question_matching/ros/src/mdr_question_matching/question_matcher.py deleted file mode 100644 index a5144eb9b..000000000 --- a/mdr_speech/mdr_question_matching/ros/src/mdr_question_matching/question_matcher.py +++ /dev/null @@ -1,112 +0,0 @@ -#!/usr/bin/env python -import rospy -import std_msgs.msg - -__author__ = 'Iryna' - - -pub_answer = rospy.Publisher("question_matcher/answer", std_msgs.msg.String, - queue_size=1) - -''' -NOTE: Questions and answers are being loaded from a txt file. -RoboCup2016: There are 50 questions given. -Give an answer to 5 out of 50 in 5 min. -''' - - -def search_items(recognized_phrase, array): - x = [] - i = '' - for item in array: - if recognized_phrase.find(item) != -1: - i = item - x.append(item) - return x - - -def sentence_matching(recognized_phrase, questions): - - candidates = [] - numbers = [] - winner = "" - - recognized_phrase = str(recognized_phrase.lower()) - words = recognized_phrase.split() - - for question in questions: - - question = str(question.lower()) - number_question = len(question.split()) - - keys = search_items(question, words) - number_keys = len(keys) - - if number_keys == number_question: # total match - # print number_keys - winner = question - return winner - - elif number_keys != number_question: - numbers.append(number_keys) - candidates.append(question) - winner = "" - - # print numbers - - if winner == "": - if max(numbers) > 0: - winner = candidates[numbers.index(max(numbers))] - else: - winner = "" - - return winner - - -def question_answer_matching(recognized_phrase): - - txtfile = rospy.get_param("~questions_file") - - questions_answers = {} - questions = [] - - with open(txtfile) as f: - for line in f: - line = line.lower() - line = line.strip() - line = line.rstrip(',') - try: - (key, val) = line.split(":") - questions_answers[str(key)] = str(val) - if str(key) != '': - questions.append(str(key)) - except ValueError: - print('Ignoring: malformed line: "{}"'.format(line)) - - winner = sentence_matching(recognized_phrase, questions) - - if not questions_answers[winner]: - # print("Unknown question!") - answer = "Sorry, I wasn't able to recognize your question!" # random answer - - elif questions_answers[winner]: - # print(questions_answers[winner]) - answer = questions_answers[winner] - - else: - print("Something bad happened...") - answer = "Sorry, I wasn't able to recognize your question!" # random answer - - return answer - - -def recognized_text_callback(recognized_phrase): - answer = question_answer_matching(recognized_phrase.data) - pub_answer.publish(answer) - - -def main(): - rospy.init_node('question_matcher', anonymous=True) - sub = rospy.Subscriber('/recognized_speech', std_msgs.msg.String, - recognized_text_callback) - rospy.spin() diff --git a/mdr_speech/mdr_rasa/CMakeLists.txt b/mdr_speech/mdr_rasa/CMakeLists.txt new file mode 100644 index 000000000..2dcb61afd --- /dev/null +++ b/mdr_speech/mdr_rasa/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mdr_rasa) + +find_package(catkin REQUIRED + COMPONENTS + rospy +) + +catkin_python_setup() + +################################### +## catkin specific configuration ## +################################### + +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES mdr_question_answering +# CATKIN_DEPENDS other_catkin_pkg +# DEPENDS system_lib +) + +############# +## Install ## +############# + +## 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} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_mdr_question_answering.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/mdr_speech/mdr_rasa/models/cocktail_party/config.yml b/mdr_speech/mdr_rasa/models/cocktail_party/config.yml new file mode 100644 index 000000000..04134c2df --- /dev/null +++ b/mdr_speech/mdr_rasa/models/cocktail_party/config.yml @@ -0,0 +1,3 @@ +language: "en" + +pipeline: "spacy_sklearn" diff --git a/mdr_speech/mdr_rasa/models/cocktail_party/training_data.md b/mdr_speech/mdr_rasa/models/cocktail_party/training_data.md new file mode 100644 index 000000000..7809f2ab7 --- /dev/null +++ b/mdr_speech/mdr_rasa/models/cocktail_party/training_data.md @@ -0,0 +1,62 @@ +## intent:tell_name +- [Henrik](name) +- Hey, my name is [Alex](name) +- Hi, I'm [Erick](name) +- I'm [Barbara](name) +- You can call me [Patricia](name) + +## intent:tell_order +- [Beer](order) +- I'd like to have a [Coke](order) +- Please bring me some [Juice](order) +- A [Water](order) please +- Could you bring me some [Vodka](order)? +- Do you have an [Energy drink](order)? + +## intent:affirm +- yes +- yep +- yeah +- indeed +- sure +- correct +- true +- great +- ok +- that's correct +- that's right +- yes, correct +- right, thank you +- sounds really good + +## intent:deny +- no +- wrong +- false +- nope +- not at all +- sorry, you misunderstood that + +## lookup:names +- Henrik +- Patrick +- Erick +- Alex +- James +- John +- Robert +- Michael +- William +- Mary +- Patricia +- Linda +- Barbara +- Elizabeth + +## lookup:orders +- Coke +- Beer +- Energy drink +- Juice +- Water +- Vodka diff --git a/mdr_speech/mdr_rasa/models/restaurant_sample/config.yml b/mdr_speech/mdr_rasa/models/restaurant_sample/config.yml new file mode 100644 index 000000000..04134c2df --- /dev/null +++ b/mdr_speech/mdr_rasa/models/restaurant_sample/config.yml @@ -0,0 +1,3 @@ +language: "en" + +pipeline: "spacy_sklearn" diff --git a/mdr_speech/mdr_rasa/models/restaurant_sample/training_data.md b/mdr_speech/mdr_rasa/models/restaurant_sample/training_data.md new file mode 100644 index 000000000..6223dce07 --- /dev/null +++ b/mdr_speech/mdr_rasa/models/restaurant_sample/training_data.md @@ -0,0 +1,63 @@ +## intent:affirm +- yes +- yep +- yeah +- indeed +- that's right +- ok +- great +- right, thank you +- correct +- great choice +- sounds really good + +## intent:goodbye +- bye +- goodbye +- good bye +- stop +- end +- farewell +- Bye bye +- have a good one + +## intent:greet +- hey +- howdy +- hey there +- hello +- hi +- good morning +- good evening +- dear sir + +## intent:restaurant_search +- i'm looking for a place to eat +- I want to grab lunch +- I am searching for a dinner spot +- i'm looking for a place in the [north](location) of town +- show me [chinese](cuisine) restaurants +- show me [chines](cuisine:chinese) restaurants in the [north](location) +- show me a [mexican](cuisine) place in the [centre](location) +- i am looking for an [indian](cuisine) spot called olaolaolaolaolaola +- search for restaurants +- anywhere in the [west](location) +- anywhere near [18328](location) +- I am looking for [asian fusion](cuisine) food +- I am looking a restaurant in [29432](location) +- I am looking for [mexican indian fusion](cuisine) +- [central](location) [indian](cuisine) restaurant + +## synonym:chinese ++ Chines +* Chinese + +## synonym:vegetarian +- vegg +- veggie + +## regex:zipcode +- [0-9]{5} + +## regex:greet +- hey[^\s]* \ No newline at end of file diff --git a/mdr_speech/mdr_rasa/models/training.sh b/mdr_speech/mdr_rasa/models/training.sh new file mode 100755 index 000000000..7bb536e0d --- /dev/null +++ b/mdr_speech/mdr_rasa/models/training.sh @@ -0,0 +1,13 @@ +#!/bin/bash + +# set CWD to the script's directory +cd "$(dirname "${BASH_SOURCE[0]}")" + +# check number of arguments +if [ "$#" -ne 1 ]; then + echo "Format: ./training.sh " + exit 1 +fi + +# train the model +python -m rasa_nlu.train -c "$1/config.yml" --data "$1/training_data.md" -o "$1" --project "generated" --fixed_model_name "model" --verbose diff --git a/mdr_speech/mdr_rasa/package.xml b/mdr_speech/mdr_rasa/package.xml new file mode 100644 index 000000000..af63efe74 --- /dev/null +++ b/mdr_speech/mdr_rasa/package.xml @@ -0,0 +1,28 @@ + + + mdr_rasa + 0.0.1 + A wrapper for the rasa framework + + MAS robotics + + Henrik Schnor + + GPLv3 + + + + + + + + + + + + + catkin + + rospy + + diff --git a/mdr_speech/mdr_rasa/ros/launch/rasa_nlu.launch b/mdr_speech/mdr_rasa/ros/launch/rasa_nlu.launch new file mode 100644 index 000000000..7e8d3d3dd --- /dev/null +++ b/mdr_speech/mdr_rasa/ros/launch/rasa_nlu.launch @@ -0,0 +1,4 @@ + + + + diff --git a/mdr_speech/mdr_rasa/ros/scripts/rasa_nlu_node b/mdr_speech/mdr_rasa/ros/scripts/rasa_nlu_node new file mode 100755 index 000000000..afbeaa626 --- /dev/null +++ b/mdr_speech/mdr_rasa/ros/scripts/rasa_nlu_node @@ -0,0 +1,6 @@ +#!/usr/bin/env python + +from mdr_rasa_nlu_wrapper.rasa_nlu_wrapper import RasaNluWrapper + +if __name__ == '__main__': + RasaNluWrapper().run() diff --git a/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_wrapper/__init__.py b/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_wrapper/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_wrapper/rasa_nlu_wrapper.py b/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_wrapper/rasa_nlu_wrapper.py new file mode 100644 index 000000000..899fc62bd --- /dev/null +++ b/mdr_speech/mdr_rasa/ros/src/mdr_rasa_nlu_wrapper/rasa_nlu_wrapper.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python + +import rospy +import os +import json +from std_msgs.msg import String +from rasa_nlu.model import Interpreter + + +class RasaNluWrapper(object): + + def __init__(self): + # Setup rospy and get config + rospy.init_node("rasa_nlu_wrapper") + self.model_name = rospy.get_param('~rasa_nlu_model', 'restaurant_sample') + self.topic_input_text = rospy.get_param('~rasa_nlu_input_topic', '/rasa_nlu_input') + self.topic_output = rospy.get_param('~rasa_nlu_output_topic', '/rasa_nlu_output') + + # Build paths to rasa models + self.base_path = os.path.abspath(os.path.join(os.path.dirname( __file__ ), '../../../models', self.model_name)) + self.model_path = os.path.join(self.base_path, 'generated', 'model') + + def run(self): + # Setup interpreter + self.interpreter = Interpreter.load(self.model_path) + + # Setup topics for incoming/outgoing messages + self.pub = rospy.Publisher(self.topic_output, String, latch=True, queue_size=1) + self.sub = rospy.Subscriber(self.topic_input_text, String, self.process_msg) + + # Wait until killed + rospy.spin() + + def get_result(self, data): + # Parse message with rasa nlu + return self.interpreter.parse(unicode(data.data, 'utf-8')) + + def process_msg(self, data): + # Process message and return result as json + result = self.get_result(data) + result_json = json.dumps(result, indent=4, separators=(',', ': ')) + rospy.loginfo("rasa_nlu output: \n" + result_json) + + # Publish result + response = String() + response.data = result_json + self.pub.publish(response) diff --git a/mdr_speech/mdr_question_matching/setup.py b/mdr_speech/mdr_rasa/setup.py similarity index 58% rename from mdr_speech/mdr_question_matching/setup.py rename to mdr_speech/mdr_rasa/setup.py index de2b39f92..5369241a2 100644 --- a/mdr_speech/mdr_question_matching/setup.py +++ b/mdr_speech/mdr_rasa/setup.py @@ -4,8 +4,8 @@ from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( - packages=['mdr_question_matching'], - package_dir={'mdr_question_matching': 'ros/src/mdr_question_matching'} + packages=['mdr_rasa_nlu_wrapper'], + package_dir={'mdr_rasa_nlu_wrapper': 'ros/src/mdr_rasa_nlu_wrapper'} ) setup(**d) diff --git a/mdr_speech/mdr_speech_recognition/ros/launch/speech_recognition.launch b/mdr_speech/mdr_speech_recognition/ros/launch/speech_recognition.launch index 2b32cadb3..7a536aba5 100644 --- a/mdr_speech/mdr_speech_recognition/ros/launch/speech_recognition.launch +++ b/mdr_speech/mdr_speech_recognition/ros/launch/speech_recognition.launch @@ -1,4 +1,8 @@ - + + + + + diff --git a/mdr_speech/mdr_speech_recognition/ros/src/mdr_speech_recognition/speech_recognizer.py b/mdr_speech/mdr_speech_recognition/ros/src/mdr_speech_recognition/speech_recognizer.py index 8501f230c..2d0abfcef 100755 --- a/mdr_speech/mdr_speech_recognition/ros/src/mdr_speech_recognition/speech_recognizer.py +++ b/mdr_speech/mdr_speech_recognition/ros/src/mdr_speech_recognition/speech_recognizer.py @@ -1,5 +1,6 @@ #!/usr/bin/env python from __future__ import print_function +import sys import rospy import httplib from std_msgs.msg import String @@ -10,7 +11,16 @@ class SpeechRecognizer(object): def __init__(self): rospy.init_node("speech_recognizer") self.pub = rospy.Publisher("speech_recognizer", String, latch=True, queue_size=1) + self.model_directory = rospy.get_param('~model_directory') + self.use_kaldi = rospy.get_param('~use_kaldi') self.recognizer = sr.Recognizer() + if self.use_kaldi: + try: + self.recognizer.load_kaldi_model(model_directory=self.model_directory) + except: + self.use_kaldi = False + rospy.logerr(sys.exc_info()[0]) + rospy.logerr('Unable to load Kaldi model. Using PocketSphinx as offline speech recognition') self.microphone = sr.Microphone() @staticmethod @@ -49,7 +59,10 @@ def recognize(self): rospy.logerr("Could not request results.") else: try: - recognized_speech = self.recognizer.recognize_sphinx(audio) + if self.use_kaldi: + recognized_speech = self.recognizer.recognize_kaldi(audio)[0] + else: + recognized_speech = self.recognizer.recognize_sphinx(audio) except sr.UnknownValueError: rospy.logerr("Could not understand audio.") except sr.RequestError: