Google Summer of Code 2016
Reactive and Adaptive Path Planning
Using Sensor Feedback and Operator Input
Last Edit: 2016-08-22 14:07:51
- Student Name: Kentaro Wada (email@example.com)
- Project Page: here
- Title: "Reactive and Adaptive Path Planning Using Sensor Feedback and Operator Input"
- Mentors: Shaun Edwards (firstname.lastname@example.org), Fred Proctor
- Organization: Open Source Robotics Foundation
Reactive and adaptive planning is necessary in situations with unknown environment, few sensors and many occlusions. Reactiveness is the mechanism to avoid collision and adaptiveness is one to complete the task. The inputs to achieve these mechanisms are sensor inputs and user operations. Based on these prerequisites, I created a system for reactive and adaptive planning as shown in below:
The part of the system consists of a simulator, but the component, Simulator, can be replaced by the real robot. The inputs are provided the simulated sensor and a user in the system. Using these inputs, the two mechanism are achieved with Stop execution and Re-planning: the former is a function for reactiveness and the latter is one for adaptiveness. As a reactiveness, the system needs to stop the joint trajectory execution of the robot to avoid collision in an environment, and as a adaptiveness the system needs to re-plan the joint trajectory to complete the task based on the information at stopping execution.
In this project I worked for the two functions, stop execution and re-planning, and also the required components in the above system. The sections afterward describes about the complete/incomplete things of this project, and the prefix in the title (eg. [Reactive], [Simulator]) represents what are the changes for.
This section provides the completed PRs/Issues in this GSoC project. Some of them are not completed by alone myself, and completed by the cooperation with maintainers of other open source projects.
[Reactive] Stop execution on motion planning rviz plugin: ros-planning/moveit_ros#709
This function is required to stop trajectory execution by operator inputs, and for the reactiveness in the system. I created a GUI to achieve this function and sent PR to one of the repos of MoveIt!. In the end, this PR is closed (not merged) and one of the maintainers provided another PR with similar changes. The reason my PR is rejected is the missing feature to disable the button until the trajectory execution is completed. The maintainer knew a simpler implementation for this than mine, and he provided the PR and merged. The result is shown in below movie:
This section provides the left PRs/Issues in this GSoC project. This means the project is not yet 100% complete. The things left to do are listed below.
[Simulator] Add package to spawn model and position control on Gazebo simulator: ros-industrial/fanuc_experimentall#20
This is a PR to simulate the robot fanuc_lrmate200id on one of the most popular simulators on robotics, Gazebo, based on closed PRs/Issues (ros-industrial/fanuc_experimental#19, ros-industrial/fanuc_experimental#22, ros-industrial/fanuc#145), for creating the whole system. The reason why the robot is chosen is that the real robot is owned by the one of the maintainers, and he can use my code with it. Gazebo is a physics simulator, so in order to spawn the model and use it, it is necessary to set the inertial values. The correct inertial values are not open (as shown in ros-industrial/fanuc_experimental#22), so I estimated it with the meshes of the links and Meshlab in a way described here. The result is shown in below figure:
[Simulator] Fix vacuum gripper plugin to work in stable: ros-simulation/gazebo_ros_pkgs#482
This is a PR to simulate the vacuum gripper in order to run manipulation tasks on the simulator. Without this PR, the vacuum gripper plugin does not work as expected because of the too fast angular velocity of the manipulating object. This PR is under reviews.
[Planner] Controlling Fanuc LRMate200iD with Moveit on Gazebo: ros-industrial/fanuc_experimental#21
This is a PR to control Fanuc LRMate200iD robot on Gazebo with Moveit!, for creatign the whole system. MoveIt! is one of the most popular project for robotic planning, and I decided to use it on this project. The result is shown in below:
[Planner] Fix namespace of params for trajectory_execution
This is a bugfix of MoveIt!, with related issues (ros-planning/moveit#61 (closed), ros-industrial/fanuc_experimental#24). Because of this bug the trajectory execution for the robot is usually canceled, and sometimes it breaks the robot on the simulation. To fix this I proposed PRs below:
[Adaptive] Sample of OctomapServerContact does not work: voxel is not updated with contact?: jsk-ros-pkg/jsk_recognition#1834
This is an bug report I proposed, but not yet fixed. The OctomapServerContact is a voxel collision object map generation method using robot contact aside from the point cloud sensed with RGB-D cameras, and this kind of mapping is necessary for re-planning after the detecting contact and stopping execution. So this is a feature for adaptiveness in the system. Currently the voxel map is not updated with the robot contact that is detected with force sensors, and it needs to be fixed.
[Adaptive] Scene visualization won't work on rviz plugin after some joint trajectory execution: ros-planning/moveit_ros#711
This is a bug report I proposed, but not yet fixed. Updating planning scene is necessary for adaptiveness on this project, to store the contact detected with the force sensors. This is included feature in MoveIt!, but not worked in some situations.
[Reactive] Add capability of executing known trajectory with a ROS action: ros-planing/moveit#94
This is a PR to execute trajectory with monitoring, based on PRs/Issues ( ros-planning/moveit#60 (wrongly merged and reverted), ros-planning/moveit_msgs#23 (closed), ros-planning/moveit_msgs#24 ros-planning/moveit_ros#719 (closed), ros-planning/moveit_msgs#27 (merged) ), for the reactiveness in the system. Monitoring trajectory execution is necessary for operator cancel GUI handling as described here. Also the function to monitor the execution is necessary for controllers for completely automated system, because the robot controller needs to select canceling execution or waiting for the completion during the execution.
This section provides some demonstrations to show what is achieved by this project.
Gazebo demonstration of reactive planning: Touch the floor: ros-industrial/fanuc_experimental#23
This demo is to show the function of reactiveness of the system. In the demo, a force sensor at the last joint of the robot detects the unexpected force input, and the controller stops the trajectory execution based on that. Without the reactiveness the robot breaks with collision to the floor, but with my system the robot successfully detects the floor. The result is shown in below movies, one is without reactiveness and the other is with it:
Pick and place demonstration with Fanuc LRMate200id in a situation with few sensors with reactive and adaptive planning: ros-industrial/fanuc_experimental#25
This demo is to show the function of reactiveness and adaptiveness of the system in a situation with few sensors, and many occlusions, with a operator input. The task is to pick object from a red bin and place it in a shelf bin. The shelf bin has many occlusions and robot needs to detect collision and stop trajectory execution if necessary. The operator can stop the trajectory execution with unexpected force input, and successfully complete the task based on the history of the contact beforehand. The result is shown in below movies, one is to show the reactiveness and the other is to show the completion of task afterwards:
For reactive and adaptive planning, functions for stopping trajectory execution and re-planning are necessary. I completed most of the features for reactiveness but there are still some open issues for re-planning: #issue_moveit_scene, #issue_octomap_contact. The demonstrations above shows the system is effective in a situation with few sensors and occlusions, but the completely automated planning using only sensor inputs not operator inputs is not yet tackled. It is one of the future works.
ROS workspace for code which is not yet merged
- Setup ROS following this link
- Setup ROS workspace with commands below:
mkdir -p ~/ros/gsoc_ws/src cd ~/ros/gsoc_ws/src wget https://raw.githubusercontent.com/wkentaro/gsoc-2016/master/rosinstall -O .rosinstall wstool up cd .. catkin build