diff --git a/.travis b/.travis index 74e968928..79d66b206 160000 --- a/.travis +++ b/.travis @@ -1 +1 @@ -Subproject commit 74e968928601f613b2cec821c1a5975baafc1127 +Subproject commit 79d66b206ca27f3a517ca5274a32381975b4c542 diff --git a/demos/selective_dualarm_grasping/.gitignore b/demos/selective_dualarm_grasping/.gitignore new file mode 100644 index 000000000..2bcdfd92b --- /dev/null +++ b/demos/selective_dualarm_grasping/.gitignore @@ -0,0 +1 @@ +models/ diff --git a/demos/selective_dualarm_grasping/CMakeLists.txt b/demos/selective_dualarm_grasping/CMakeLists.txt new file mode 100644 index 000000000..168f278bf --- /dev/null +++ b/demos/selective_dualarm_grasping/CMakeLists.txt @@ -0,0 +1,42 @@ +cmake_minimum_required(VERSION 2.8.3) +project(dualarm_grasping) + +find_package(catkin REQUIRED + dynamic_reconfigure + jsk_recognition_msgs + message_generation + roseus + std_msgs +) + +catkin_python_setup() + +add_message_files( + FILES + GraspClassificationResult.msg +) + +add_service_files( + FILES + GetAnother.srv +) + +generate_messages( + DEPENDENCIES + std_msgs + jsk_recognition_msgs +) + +# add_custom_target(${PROJECT_NAME}_install_models ALL COMMAND ${PROJECT_SOURCE_DIR}/scripts/install_models.py) + +generate_dynamic_reconfigure_options( + cfg/DualarmGraspSegmentation.cfg + cfg/DualarmOccludedGraspInstanceSegmentation.cfg +) + +catkin_package( + CATKIN_DEPENDS + std_msgs + jsk_recognition_msgs + message_runtime +) diff --git a/demos/selective_dualarm_grasping/README.md b/demos/selective_dualarm_grasping/README.md new file mode 100644 index 000000000..59e016e04 --- /dev/null +++ b/demos/selective_dualarm_grasping/README.md @@ -0,0 +1,155 @@ +# Advanced Robotics 2020: Selective dual-arm occluded grasping + +## Download models + +```bash +rosrun dualarm_grasping install_models.py +``` + +## Sampling + +### Setup + +```bash +roslaunch dualarm_grasping baxter.launch +roslaunch dualarm_grasping setup_occluded_sampling.launch +``` + +Pass args for `first_sampling:=true` or `second_sampling:=true` + +### Main Program + +```bash +roseus euslisp/sampling-grasp.l +``` + +## Execution (Cluster Grasping) + +### Setup + +```bash +roslaunch dualarm_grasping baxter.launch +roslaunch dualarm_grasping setup_occluded_picking.launch +``` + +### Main Program + +```bash +roseus euslisp/dualarm-grasp.l +``` + +## Execution (Target Grasping) + +### Setup + +```bash +roslaunch dualarm_grasping baxter.launch +roslaunch dualarm_grasping setup_occluded_picking.launch target_grasp:=true +``` + +### Main Program + +```bash +roseus euslisp/dualarm-grasp.l +``` + +## Evaluation (Target Grasping) + +### Setup + +```bash +roslaunch dualarm_grasping baxter.launch +roslaunch dualarm_grasping setup_occluded_test.launch target_grasp:=true +``` + +### Main Program + +```bash +roseus euslisp/test-grasp.l +``` + +## Citation + +```bib +@article{doi:10.1080/01691864.2020.1783352, + author = {Shingo Kitagawa and Kentaro Wada and Shun Hasegawa and Kei Okada and Masayuki Inaba}, + title = {Few-experiential learning system of robotic picking task with selective dual-arm grasping}, + journal = {Advanced Robotics}, + volume = {0}, + number = {0}, + pages = {1-19}, + year = {2020}, + publisher = {Taylor & Francis}, + doi = {10.1080/01691864.2020.1783352}, + URL = {https://doi.org/10.1080/01691864.2020.1783352}, + eprint = {https://doi.org/10.1080/01691864.2020.1783352} +} +``` + +# IROS2018: Selective dual-arm grasping + +## Download models + +```bash +rosrun dualarm_grasping install_models.py +``` + +## Sampling + +### Setup + +```bash +roslaunch dualarm_grasping baxter.launch +roslaunch dualarm_grasping setup_sampling.launch +``` + +Pass args for `first_sampling:=true` or `second_sampling:=true` + +### Main Program + +```bash +roseus euslisp/sampling-grasp.l +``` + +## Execution + +### Setup + +```bash +roslaunch dualarm_grasping baxter.launch +roslaunch dualarm_grasping setup_picking.launch +``` + +### Main Program + +```bash +roseus euslisp/dualarm-grasp.l +``` + +## Evaluation + +### Setup + +```bash +roslaunch dualarm_grasping baxter.launch +roslaunch dualarm_grasping setup_test.launch +``` + +### Main Program + +```bash +roseus euslisp/test-grasp.l +``` + +## Citation + +```bib +@inproceedings{SelectiveDualarmGrasping:IROS2018, + author={Shingo Kitagawa and Kentaro Wada and Shun Hasegawa and Kei Okada and Masayuki Inaba}, + booktitle={Proceedings of The 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems}, + title={Multi-stage Learning of Selective Dual-arm Grasping Based on Obtaining and Pruning Grasping Points Through the Robot Experience in the Real World}, + year={2018}, + month={october}, + pages={7123-7130}, +} +``` diff --git a/demos/selective_dualarm_grasping/cfg/DualarmGraspSegmentation.cfg b/demos/selective_dualarm_grasping/cfg/DualarmGraspSegmentation.cfg new file mode 100755 index 000000000..33d9442d5 --- /dev/null +++ b/demos/selective_dualarm_grasping/cfg/DualarmGraspSegmentation.cfg @@ -0,0 +1,17 @@ +#!/usr/bin/env python +from dynamic_reconfigure.parameter_generator_catkin import double_t +from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator +from dynamic_reconfigure.parameter_generator_catkin import str_t + + +PACKAGE = "dualarm_grasping" + + +gen = ParameterGenerator() + +gen.add('score_thresh', double_t, 0, 'Score thresh', 0.5, 0.0, 1.0) +gen.add('grasp_thresh', double_t, 0, 'Grasp Score thresh', 0.3, 0.0, 1.0) +gen.add('sampling_thresh', double_t, 0, 'Sampling Score thresh', 0.7, 0.0, 1.0) +gen.add('grasping_way', str_t, 0, 'Grasping way', 'single') + +exit(gen.generate(PACKAGE, PACKAGE, 'DualarmGraspSegmentation')) diff --git a/demos/selective_dualarm_grasping/cfg/DualarmOccludedGraspInstanceSegmentation.cfg b/demos/selective_dualarm_grasping/cfg/DualarmOccludedGraspInstanceSegmentation.cfg new file mode 100755 index 000000000..46dc0baae --- /dev/null +++ b/demos/selective_dualarm_grasping/cfg/DualarmOccludedGraspInstanceSegmentation.cfg @@ -0,0 +1,20 @@ +#!/usr/bin/env python +from dynamic_reconfigure.parameter_generator_catkin import double_t +from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator +from dynamic_reconfigure.parameter_generator_catkin import str_t + + +PACKAGE = "dualarm_grasping" + + +gen = ParameterGenerator() + +gen.add('score_thresh', double_t, 0, 'Score thresh', 0.5, 0.0, 1.0) +gen.add('grasp_thresh', double_t, 0, 'Grasp Score thresh', 0.3, 0.0, 1.0) +gen.add('nms_thresh', double_t, 0, 'NMS thresh', 0.3, 0.0, 1.0) +gen.add('vis_thresh', double_t, 0, 'Vis region ratio thresh', 0.8, 0.0, 1.0) +gen.add('sampling_thresh', double_t, 0, 'Sampling Score thresh', 0.7, 0.0, 1.0) +gen.add('grasping_way', str_t, 0, 'Grasping way', 'single') + +exit(gen.generate( + PACKAGE, PACKAGE, 'DualarmOccludedGraspInstanceSegmentation')) diff --git a/demos/selective_dualarm_grasping/config/baxter/occlusion_dataset/tote_marker.yaml b/demos/selective_dualarm_grasping/config/baxter/occlusion_dataset/tote_marker.yaml new file mode 100644 index 000000000..61b11e176 --- /dev/null +++ b/demos/selective_dualarm_grasping/config/baxter/occlusion_dataset/tote_marker.yaml @@ -0,0 +1,7 @@ +boxes: +- dimensions: [0.35, 0.6, 0.3] + frame_id: base + name: tote + orientation: [0.006811395297589589, -0.02481966841912109, 0.014732206262089195, + 0.9995601788048162] + position: [0.6535828113555908, 0.11746583878993988, -0.3259793519973755] diff --git a/demos/selective_dualarm_grasping/config/baxter/target_grasp/target_tote_marker.yaml b/demos/selective_dualarm_grasping/config/baxter/target_grasp/target_tote_marker.yaml new file mode 100644 index 000000000..5f1b51566 --- /dev/null +++ b/demos/selective_dualarm_grasping/config/baxter/target_grasp/target_tote_marker.yaml @@ -0,0 +1,7 @@ +boxes: +- dimensions: [0.3499999940395355, 0.6000000238418579, 0.20999999344348907] + frame_id: base + name: target_tote + orientation: [0.0070683994862820645, -0.02474772236333151, 0.004366822631185315, + 0.9996592013414559] + position: [1.0436279773712158, 0.029123149812221527, -0.342985063791275] diff --git a/demos/selective_dualarm_grasping/config/baxter/target_grasp/tote_marker.yaml b/demos/selective_dualarm_grasping/config/baxter/target_grasp/tote_marker.yaml new file mode 100644 index 000000000..26aae08dd --- /dev/null +++ b/demos/selective_dualarm_grasping/config/baxter/target_grasp/tote_marker.yaml @@ -0,0 +1,7 @@ +boxes: +- dimensions: [0.35, 0.6, 0.21] + frame_id: base + name: tote + orientation: [-0.005413727427457605, -0.02493861717018956, 0.011270340653429979, + 0.9996107924340031] + position: [0.6436677575111389, 0.01432156190276146, -0.4225315749645233] diff --git a/demos/selective_dualarm_grasping/config/baxter/target_tote_marker.yaml b/demos/selective_dualarm_grasping/config/baxter/target_tote_marker.yaml new file mode 100644 index 000000000..a7ed12f43 --- /dev/null +++ b/demos/selective_dualarm_grasping/config/baxter/target_tote_marker.yaml @@ -0,0 +1,7 @@ +boxes: +- dimensions: [0.3499999940395355, 0.6000000238418579, 0.30000001192092896] + frame_id: base + name: target_tote + orientation: [0.007238342304459111, -0.024698547601765104, -0.002504836958434969, + 0.9996656010581031] + position: [1.0950188636779785, 0.032183099538087845, -0.35149049758911133] diff --git a/demos/selective_dualarm_grasping/config/baxter/tote_marker.yaml b/demos/selective_dualarm_grasping/config/baxter/tote_marker.yaml new file mode 100644 index 000000000..94bba587d --- /dev/null +++ b/demos/selective_dualarm_grasping/config/baxter/tote_marker.yaml @@ -0,0 +1,7 @@ +boxes: +- dimensions: [0.30000001192092896, 0.5, 0.30000001192092896] + frame_id: base + name: tote + orientation: [0.007238342304459111, -0.024698547601765104, -0.002504836958434969, + 0.9996656010581031] + position: [0.7619464993476868, 0.021540701389312744, -0.4418632388114929] diff --git a/demos/selective_dualarm_grasping/config/pr2/target_grasp/target_tote_marker.yaml b/demos/selective_dualarm_grasping/config/pr2/target_grasp/target_tote_marker.yaml new file mode 100644 index 000000000..111c9d753 --- /dev/null +++ b/demos/selective_dualarm_grasping/config/pr2/target_grasp/target_tote_marker.yaml @@ -0,0 +1,7 @@ +boxes: +- dimensions: [0.35, 0.6, 0.21] + frame_id: base_link + name: target_tote + orientation: [0.0070683994862820645, -0.02474772236333151, 0.004366822631185315, + 0.9996592013414559] + position: [1.0621156692504883, 0.023430507630109787, -0.35995131731033325] diff --git a/demos/selective_dualarm_grasping/config/pr2/target_grasp/tote_marker.yaml b/demos/selective_dualarm_grasping/config/pr2/target_grasp/tote_marker.yaml new file mode 100644 index 000000000..1203028c8 --- /dev/null +++ b/demos/selective_dualarm_grasping/config/pr2/target_grasp/tote_marker.yaml @@ -0,0 +1,7 @@ +boxes: +- dimensions: [0.35, 0.6, 0.21] + frame_id: base_link + name: tote + orientation: [0.006889725010306631, -0.02479804646037768, 0.011576424195440006, + 0.9996017081737498] + position: [0.7192068696022034, 0.018723100423812866, -0.37579140067100525] diff --git a/demos/selective_dualarm_grasping/config/pr2/target_tote_marker.yaml b/demos/selective_dualarm_grasping/config/pr2/target_tote_marker.yaml new file mode 100644 index 000000000..cecd2ed87 --- /dev/null +++ b/demos/selective_dualarm_grasping/config/pr2/target_tote_marker.yaml @@ -0,0 +1,7 @@ +boxes: +- dimensions: [0.3499999940395355, 0.6000000238418579, 0.30000001192092896] + frame_id: base_link + name: target_tote + orientation: [0.007238342304459111, -0.024698547601765104, -0.002504836958434969, + 0.9996656010581031] + position: [1.0213091373443604, 0.016158416867256165, 0.7774733304977417] diff --git a/demos/selective_dualarm_grasping/config/pr2/tote_marker.yaml b/demos/selective_dualarm_grasping/config/pr2/tote_marker.yaml new file mode 100644 index 000000000..ce8e3d00f --- /dev/null +++ b/demos/selective_dualarm_grasping/config/pr2/tote_marker.yaml @@ -0,0 +1,7 @@ +boxes: +- dimensions: [0.30000001192092896, 0.5, 0.30000001192092896] + frame_id: base_link + name: tote + orientation: [0.007184166449114169, -0.045677434576014746, -0.00265622790454915, + 0.9989268762909047] + position: [0.6855822801589966, 0.04986734688282013, 0.7560162544250488] diff --git a/demos/selective_dualarm_grasping/data/json/item_location_file.json b/demos/selective_dualarm_grasping/data/json/item_location_file.json new file mode 100644 index 000000000..e7b79215b --- /dev/null +++ b/demos/selective_dualarm_grasping/data/json/item_location_file.json @@ -0,0 +1,30 @@ +{ + "bins": [ + { + "bin_id": "A", + "contents": [] + }, + { + "bin_id": "B", + "contents": [] + }, + { + "bin_id": "C", + "contents": [] + } + ], + "boxes": [], + "tote": { + "contents": [ + "avery_binder", + "composition_book", + "hanes_socks", + "ice_cube_tray", + "reynolds_wrap", + "robots_dvd", + "scotch_sponges", + "table_cloth", + "toilet_brush" + ] + } +} diff --git a/demos/selective_dualarm_grasping/euslisp/dualarm-grasp.l b/demos/selective_dualarm_grasping/euslisp/dualarm-grasp.l new file mode 100755 index 000000000..0b0f7738d --- /dev/null +++ b/demos/selective_dualarm_grasping/euslisp/dualarm-grasp.l @@ -0,0 +1,155 @@ +#!/usr/bin/env roseus + +(ros::roseus "robot_main") +(require "package://dualarm_grasping/euslisp/lib/dualarm-grasp-interface.l") +;; smach +(require :state-machine "package://roseus_smach/src/state-machine.l") +(require :state-machine-ros "package://roseus_smach/src/state-machine-ros.l") +(require :state-machine-utils "package://roseus_smach/src/state-machine-utils.l") + +(make-random-state t) + + +(defun make-dualarm-grasp-state-machine () + (setq *sm* + (make-state-machine + '((:init -> :recognize-bboxes) + (:recognize-bboxes -> :wait-for-user-input) + (:wait-for-user-input -> :recognize-object) + (:wait-for-user-input !-> :finish) + (:recognize-object -> :pick-object) + (:recognize-object !-> :recognize-object) + (:recognize-object :give-up :wait-for-user-input) + (:pick-object -> :verify-object) + (:pick-object !-> :recognize-object) + (:verify-object -> :place-object) + (:verify-object !-> :return-object) + (:verify-object :no-target :remove-object) + (:place-object -> :recognize-object) + (:return-object -> :recognize-object) + (:remove-object -> :recognize-object)) + '((:init + '(lambda (userdata) + (send *ti* :reset-giveup :larm) + (send *ti* :reset-giveup :rarm) + (ros::ros-info-green "[dualarm-grasp] start picking") + t)) + (:recognize-bboxes + '(lambda (userdata) + (send *ti* :recognize-bboxes) + t)) + (:wait-for-user-input + '(lambda (userdata) + (send *ti* :wait-for-user-input))) + (:recognize-object + '(lambda (userdata) + (let ((arm (elt '(:larm :rarm) (random 2))) + success-p) + (setq arm (send *ti* :set-recognize-arm arm :update t)) + (unix::sleep 1) + (setq success-p + (send *ti* :recognize-object arm)) + (if success-p + (progn + (send *ti* :set-fail-recognize-arm nil) + (send *ti* :set-save-dir) + (send *ti* :recognition-save-request)) + (progn + ; (send *ti* :giveup) + (send *ti* :set-fail-recognize-arm arm) + (send *ti* :return-from-recognize-object arm))) + (setf (cdr (assoc 'grasp-arm userdata)) + (send *ti* :get-grasp-arm arm)) + success-p))) + (:pick-object + '(lambda (userdata) + (let (pick-result success-p (arm (cdr (assoc 'grasp-arm userdata)))) + (send *ti* :calib-prismatic-joint arm) + (setq pick-result (send *ti* :pick-object arm)) + (setq success-p (send *ti* :graspingp arm)) + (unless success-p + (send *ri* :stop-grasp arm) + (ros::ros-info-green "[dualarm-grasp] pick failed because of ~A" pick-result) + (send *ti* :return-from-pick-object arm) + ; (send *ti* :giveup) + ) + (send *ti* :set-result pick-result) + (send *ti* :result-save-request) + (if (or (eq pick-result :moveit-failed) (eq pick-result :ik-failed)) + (send *ti* :set-fail-recognize-arm (send *ti* :get-recognize-arm))) + success-p))) + (:verify-object + '(lambda (userdata) + (ros::ros-info-green "[dualarm-grasp] is it target?: ~A" (send *ti* :get-is-target)) + (if (send *ti* :verify-object) + (if (send *ti* :get-is-target) t :no-target) + nil))) + (:place-object + '(lambda (userdata) + (let ((arm (cdr (assoc 'grasp-arm userdata))) + (target-grasp (cdr (assoc 'target-grasp userdata)))) + (if target-grasp + (send *ti* :place-object arm :distance 300) + (send *ti* :place-object arm)) + (send *ti* :return-from-place-object arm) + (send *ti* :calib-prismatic-joint arm)) + t)) + (:return-object + '(lambda (userdata) + (let ((arm (cdr (assoc 'grasp-arm userdata)))) + (send *ti* :return-object arm) + (send *ti* :return-from-pick-object arm) + (send *ti* :calib-prismatic-joint arm) + ; (send *ti* :giveup) + ) + t)) + (:remove-object + '(lambda (userdata) + (let ((arm (cdr (assoc 'grasp-arm userdata))) + (target-grasp (cdr (assoc 'target-grasp userdata)))) + (if target-grasp + (send *ti* :place-object arm :opposite t :distance 300) + (send *ti* :place-object arm :opposite t)) + (send *ti* :return-from-place-object arm) + (send *ti* :calib-prismatic-joint arm)) + t))) + '(:init) + '(:finish)))) + + +(defun dualarm-grasp-init (&key (ctype :default-controller) (calib-pressure t) + (moveit t) (scale nil)) + (dualarm_grasping::dualarm-grasp-init :ctype ctype :moveit moveit :scale scale) + (when moveit + (send *ti* :wipe-all-scene) + (send *ti* :add-workspace-scene)) + (send *ri* :gripper-servo-on) + ;; initialize fingers + (send *ri* :move-hand :rarm + (send *baxter* :hand-grasp-pre-pose :rarm :cylindrical) 1000 :wait nil) + (send *ri* :move-hand :larm + (send *baxter* :hand-grasp-pre-pose :larm :cylindrical) 1000) + (send *ri* :move-hand :rarm + (send *baxter* :hand-grasp-pre-pose :rarm :opposed) 1000 :wait nil) + (send *ri* :move-hand :larm + (send *baxter* :hand-grasp-pre-pose :larm :opposed) 1000) + (send *ri* :angle-vector (send *baxter* :arc-reset-pose)) + (send *ri* :wait-interpolation) + (when calib-pressure + (send *ri* :calib-pressure-threshold :rarm) + (send *ri* :calib-pressure-threshold :larm)) + (objects (list *baxter*)) + t) + + +(defun dualarm-grasp-mainloop (&key (target-grasp nil)) + (when (not (boundp '*sm*)) + (make-dualarm-grasp-state-machine)) + (exec-state-machine *sm* `((grasp-arm . :larm) + (target-grasp . ,target-grasp)) :hz 2.0)) + + +(warn "~% Commands ~%") +(warn "(dualarm-grasp-init) : initialize *ti*~%") +(warn "(dualarm-grasp-mainloop) : start random grasping mainloop~%") +(warn "(dualarm-grasp-mainloop :target-grasp t) : start target grasping mainloop~%") diff --git a/demos/selective_dualarm_grasping/euslisp/lib/dualarm-grasp-interface.l b/demos/selective_dualarm_grasping/euslisp/lib/dualarm-grasp-interface.l new file mode 100644 index 000000000..d17a3998e --- /dev/null +++ b/demos/selective_dualarm_grasping/euslisp/lib/dualarm-grasp-interface.l @@ -0,0 +1,729 @@ +#!/usr/bin/env roseus + +(require "package://jsk_2015_05_baxter_apc/euslisp/lib/util.l") +(require "package://jsk_2016_01_baxter_apc/euslisp/lib/util.l") +(require "package://jsk_arc2017_baxter/euslisp/lib/stow-interface.l") + +(ros::load-ros-manifest "dualarm_grasping") + +(unless (find-package "DUALARM_GRASPING") + (make-package "DUALARM_GRASPING")) + +(defclass dualarm_grasping::dualarm-grasp-interface + :super jsk_arc2017_baxter::stow-interface + :slots (grasping-way- + cls-result- + fail-recognize-arm- + is-target- + recognize-arm- + tote-cube- + target-label- + target-tote-cube- + use-scale-)) + +(defmethod dualarm_grasping::dualarm-grasp-interface + (:init (&key (moveit t) (scale nil)) + (send-super :init :moveit moveit) + (setq grasp-style- :suction) + (setq grasping-way- :dual) + (setq use-scale- scale)) + (:recognize-bboxes () + (ros::ros-info "[main] recognizing target tote") + (send self :recognize-target-tote-box :stamp (ros::time-now)) + (ros::ros-info "[main] recognizing tote") + (send self :recognize-tote-box :stamp (ros::time-now))) + (:recognize-tote-box + (&key (stamp (ros::time-now))) + (let ((bbox-topic "/transformable_tote_markers/output/boxes") + bbox-msg bbox) + (setq bbox-msg (one-shot-subscribe bbox-topic + jsk_recognition_msgs::BoundingBoxArray + :timeout 10000 + :after-stamp stamp)) + (if bbox-msg + (progn + (ros::ros-info "[~a] [:recognize-tote-box] recognize tote bbox" (ros::get-name)) + (setq bbox (car (send bbox-msg :boxes))) + (setq tote-cube- (send self :bbox->cube bbox))) + (ros::ros-fatal "[:recognize-tote-box] cannot recognize tote bbox")))) + (:recognize-target-tote-box + (&key (stamp (ros::time-now))) + (let ((bbox-topic "/transformable_target_tote_markers/output/boxes") + bbox-msg bbox) + (setq bbox-msg (one-shot-subscribe bbox-topic + jsk_recognition_msgs::BoundingBoxArray + :timeout 10000 + :after-stamp stamp)) + (if bbox-msg + (progn + (ros::ros-info "[~a] [:recognize-target-tote-box] recognize target tote bbox" (ros::get-name)) + (setq bbox (car (send bbox-msg :boxes))) + (setq target-tote-cube- (send self :bbox->cube bbox))) + (ros::ros-fatal "[:recognize-target-tote-box] cannot recognize target tote bbox")))) + (:add-tote-scene () + (let ((base-name (send (send *baxter* :base_lk) :name))) + (send *co* :add-object tote-cube- :frame-id base-name + :relative-pose (send tote-cube- :copy-worldcoords) + :object-id "tote"))) + (:delete-tote-scene () + (send *co* :delete-object tote-cube-)) + (:add-target-tote-scene () + (let ((base-name (send (send *baxter* :base_lk) :name))) + (send *co* :add-object target-tote-cube- :frame-id base-name + :relative-pose (send target-tote-cube- :copy-worldcoords) + :object-id "target_tote"))) + (:delete-target-tote-scene () + (send *co* :delete-object target-tote-cube-)) + (:wait-for-user-input () + (let (can-start) + (when moveit-p- + (send self :add-target-tote-scene) + (send self :add-tote-scene)) + (send *ri* :angle-vector (send *baxter* :arc-reset-pose) 3000 nil 0) + (ros::ros-info "[:wait-for-user-input] wait for user input to start") + (ros::wait-for-service "/rviz/yes_no_button") + (setq can-start + (send (ros::service-call "/rviz/yes_no_button" + (instance jsk_gui_msgs::YesNoRequest)) + :yes)) + (ros::ros-info "[:wait-for-user-input] received user input") + (setq start-time- (ros::time-now)) + can-start)) + (:recognize-object (arm &key (trial-times 5)) + (let (is-recognized recognition-count) + (setq trial-fail-count- 0) + (setq label-names + (ros::get-param (format nil "/~a_hand_camera/dualarm_grasp_segmentation/label_names" + (arm2str arm)))) + (send *ri* :move-hand arm + (send *baxter* :hand-grasp-pre-pose arm :opposed) 1000 :wait nil) + (ros::ros-info "[main] Recognizing objects in tote") + (unless (> start-picking-fail-count- 0) + (send self :move-arm-body->tote-overlook-pose arm) + (send *ri* :wait-interpolation)) + (setq recognition-count 1) + (let ((stamp (ros::time-now))) + (while (null (or (> recognition-count trial-times) is-recognized)) + (setq is-recognized + (send self :recognize-target-object arm :stamp stamp + :timeout (* recognition-count 10))) + (when (and is-recognized + (eq grasping-way- :dual) + (< (length (gethash arm object-boxes-)) 2)) + (setq is-recognized nil)) + (incf recognition-count))) + (when is-recognized + (setq target-label- (elt (send cls-result- :labels) 0)) + (setq target-obj- (elt label-names target-label-))) + is-recognized)) + (:move-arm-body->tote-overlook-pose + (arm &key (gripper-angle 90)) + (let (avs offset rpy + (offset-x (if (eq arm :larm) 0 -0)) + (offset-y (if (eq arm :larm) 250 -250))) + (setq avs (list (send *baxter* :avoid-shelf-pose arm (if (eq arm :larm) :d :f)))) + (send *baxter* :reset-pose arm) + (send *baxter* :rotate-gripper arm gripper-angle :relative nil) + (setq offset (float-vector offset-x offset-y 250)) + (setq rpy (float-vector 0 pi/2 (if (eq arm :larm) pi/2 -pi/2))) + (pushback + (send self :ik->tote-center arm + :offset offset :rpy rpy :use-gripper nil) + avs) + (send *ri* :angle-vector-raw (send *baxter* :angle-vector) 3000 + (send *ri* :get-arm-controller arm) 0))) + (:ik->tote-center + (arm &key (offset #f(0 0 0)) (rpy #f(0 0 0)) + (rotation-axis t) (use-gripper nil) (move-palm-end nil)) + (let (tote-coords) + (setq tote-coords (send tote-cube- :copy-worldcoords)) + (send tote-coords :translate + (float-vector 0.0 0.0 (/ (z-of-cube tote-cube-) 2.0)) + :local) + (send tote-coords :translate offset :world) + (send tote-coords :rotate (aref rpy 0) :z) + (send tote-coords :rotate (aref rpy 1) :y) + (send tote-coords :rotate (aref rpy 2) :x) + (send *baxter* arm :inverse-kinematics tote-coords + :rotation-axis rotation-axis + :use-gripper use-gripper + :move-palm-end move-palm-end))) + (:recognize-target-object + (arm &key (stamp (ros::time-now)) (timeout 10)) + (let ((box-topic + (format nil "~a_hand_camera/cluster_indices_decomposer/boxes" (arm2str arm))) + (com-topic + (format nil "~a_hand_camera/cluster_indices_decomposer/centroid_pose_array" (arm2str arm))) + (cls-topic + (format nil "~a_hand_camera/dualarm_grasp_segmentation/output/grasp_class" (arm2str arm))) + box-msg com-msg cls-msg obj-box obj-coords cls-style is-recognized) + (ros::subscribe box-topic jsk_recognition_msgs::BoundingBoxArray + #'(lambda (msg) + (let ((st (send msg :header :stamp))) + (when (> (send st :to-sec) (send stamp :to-sec)) + (setq box-msg msg))))) + (ros::subscribe com-topic geometry_msgs::PoseArray + #'(lambda (msg) + (let ((st (send msg :header :stamp))) + (when (> (send st :to-sec) (send stamp :to-sec)) + (setq com-msg msg))))) + (ros::subscribe cls-topic dualarm_grasping::GraspClassificationResult + #'(lambda (msg) + (let ((st (send msg :header :stamp))) + (when (> (send st :to-sec) (send stamp :to-sec)) + (setq cls-msg msg))))) + (while (and (not (and com-msg cls-msg box-msg + (if (equal (send cls-msg :style) "single") + (and (> (length (send box-msg :boxes)) 0) + (> (length (send com-msg :poses)) 0)) + (and (> (length (send box-msg :boxes)) 1) + (> (length (send com-msg :poses)) 1))))) + (> (+ (send stamp :to-sec) timeout) (send (ros::time-now) :to-sec))) + (unix::usleep (* 50 1000)) + (ros::spin-once)) + (ros::unsubscribe box-topic) + (ros::unsubscribe com-topic) + (ros::unsubscribe cls-topic) + (cond + ((and box-msg com-msg cls-msg) + (ros::ros-info "[:recognize-target-object] arm: ~a get cpi msg" arm) + (setq obj-box (send box-msg :boxes)) + (setq obj-coords + (mapcar #'(lambda (obj-pose) + (send *ri* :tf-pose->coords + (send com-msg :header :frame_id) obj-pose)) + (send com-msg :poses))) + (if (and (> (length obj-box) 0) (> (length obj-coords) 0)) + (progn + (sethash arm object-boxes- obj-box) + (sethash arm object-coords- obj-coords) + (setq is-recognized t)) + (progn + (ros::ros-error "[:recognize-target-object] arm: ~a obj-box length ~a" arm (length obj-box)) + (ros::ros-error "[:recognize-target-object] arm: ~a obj-coords length ~a" arm (length obj-coords)) + (setq is-recognized nil))) + (setq cls-style (send cls-msg :style)) + (setq grasping-way- + (cond ((equal cls-style "dual") :dual) + ((equal cls-style "single") :single) + (t nil))) + (setq is-target- (send cls-msg :is_target)) + (setq cls-result- (send cls-msg :classification))) + (t + (ros::ros-error "[:recognize-target-object] arm: ~a failed to get cpi msg" arm) + (ros::ros-error "[:recognize-target-object] arm: ~a box-msg: ~a, com-msg ~a, cls-msg ~a" + arm (if box-msg t nil) (if com-msg t nil) (if cls-msg t nil)) + (setq is-recognized nil))) + is-recognized)) + (:set-fail-recognize-arm (arm) + (setq fail-recognize-arm- arm)) + (:set-recognize-arm (arm &key (update nil)) + (if (and update fail-recognize-arm-) + (setq recognize-arm- (if (eq fail-recognize-arm- :larm) :rarm :larm)) + (setq recognize-arm- arm)) + recognize-arm-) + (:get-is-target () is-target-) + (:get-recognize-arm () recognize-arm-) + (:get-grasp-arm (arm) + (cond ((eq grasping-way- :dual) :arms) + ((eq grasping-way- :single) arm) + (t nil))) + (:pick-object (arm) + (send *ri* :speak + (format nil "robot is picking ~a." (underscore-to-space target-obj-))) + (ros::ros-info-green "grasping-way: ~A, object: ~A" grasping-way- target-obj-) + (let (pick-result graspingp) + (setq recognize-fail-count- 0) + (when moveit-p- (send self :delete-tote-scene)) + (send self :set-movable-region-for-tote :larm :offset (list 50 50 0)) + (send self :set-movable-region-for-tote :rarm :offset (list 50 50 0)) + (when use-scale- (send self :reset-scale arm)) + (send *baxter* :head_pan :joint-angle + (cond ((eq arm :larm) -70) + ((eq arm :rarm) 70) + ((eq arm :arms ) 0))) + (setq grasp-style- :suction) + (setq pick-result + (send self :pick-object-in-tote arm + :n-trial 2 + :n-trial-same-pos 1 + :do-stop-grasp nil + :grasp-style grasp-style-)) + (when moveit-p- (send self :add-tote-scene)) + ;; Don't trust pressure sensor + ;; (unless (eq pick-result :ik-failed) + ;; (setq graspingp (send *ri* :graspingp arm grasp-style-)) + ;; (unless graspingp (return-from :pick-object nil)) + ;; ) + (if (eq arm :arms) + (progn + (send *baxter* :larm :move-end-pos #f(0 0 100) :world :rotation-axis t) + (send *baxter* :rarm :move-end-pos #f(0 0 100) :world :rotation-axis t)) + (send *baxter* arm :move-end-pos #f(0 0 100) :world :rotation-axis t)) + (send *ri* :angle-vector-raw (send *baxter* :angle-vector) 3000 nil 0) + (send *ri* :wait-interpolation) + (unless (eq pick-result :grasp-succeeded) + (send *ri* :stop-grasp arm) + (return-from :pick-object pick-result)) + ;; Don't trust pressure sensor + ;; (setq graspingp (send *ri* :graspingp arm grasp-style-)) + ;; (ros::ros-info "[main] arm: ~a graspingp: ~a" arm graspingp) + ;; graspingp)) + (if use-scale- + (progn + (setq scale-candidates- + (send self :get-scale-candidates arm :picked :stamp (ros::time-now))) + ;; If scale value isn't changed, we don't have to verify-object and return-object + (if (eq scale-candidates- :no-change) + (progn + (ros::ros-info-green "[main] arm ~a: grasping nothing is detected by scale" arm) + nil) pick-result)) + pick-result))) + (:set-movable-region-for-tote + (arm &key (offset (list 0 0 0))) + (sethash arm tote-movable-regions- + (send self :cube->movable-region tote-cube- :offset offset))) + (:pick-object-in-tote (arm &rest args) + (let (pick-result) + (setq pick-result + (if (eq arm :arms) + (send* self :dualarm-pick-object-in-tote args) + (send-super* :pick-object-in-tote arm args))) + pick-result)) + (:dualarm-pick-object-in-tote + (&key (n-trial 1) (n-trial-same-pos 1) (do-stop-grasp nil) (grasp-style :suction)) + (let (pick-result movable-region + larm-obj-pos larm-obj-cube larm-object-index + rarm-obj-pos rarm-obj-cube rarm-object-index) + (setq larm-movable-region (gethash :larm tote-movable-regions-)) + (setq rarm-movable-region (gethash :rarm tote-movable-regions-)) + (unless (and rarm-movable-region larm-movable-region) + (ros::ros-error "[:dualarm-pick-object-in-tote] No movable region for tote. Call :set-movable-region-for-tote first.") + (return-from :dualarm-pick-object-in-tote nil)) + (unless (> (length (gethash recognize-arm- object-coords-)) 1) + (return-from :dualarm-pick-object-in-tote nil)) + (unless (> (length (gethash recognize-arm- object-boxes-)) 1) + (return-from :dualarm-pick-object-in-tote nil)) + (if (> (elt (send (elt (gethash recognize-arm- object-coords-) 0) :pos) 1) + (elt (send (elt (gethash recognize-arm- object-coords-) 1) :pos) 1)) + (setq larm-object-index 0 + rarm-object-index 1) + (setq larm-object-index 1 + rarm-object-index 0)) + (setq larm-obj-pos + (send self :get-object-position recognize-arm- + larm-movable-region :object-index larm-object-index)) + (setq larm-obj-cube + (send self :bbox->cube + (elt (gethash recognize-arm- object-boxes-) larm-object-index))) + (setq rarm-obj-pos + (send self :get-object-position recognize-arm- + rarm-movable-region :object-index rarm-object-index)) + (setq rarm-obj-cube + (send self :bbox->cube + (elt (gethash recognize-arm- object-boxes-) rarm-object-index))) + (send *ri* :stop-grasp :larm :pinch) + (send *ri* :stop-grasp :rarm :pinch) + (send *ri* :calib-proximity-threshold :larm) + (send *ri* :calib-proximity-threshold :rarm) + (send *ri* :gripper-servo-on :larm) + (send *ri* :gripper-servo-on :rarm) + (dotimes (i n-trial) + (dotimes (j n-trial-same-pos) + (when (or (null pick-result) (null (eq pick-result :grasp-succeeded))) + (setq pick-result + (send self :dualarm-try-to-pick-object larm-obj-pos rarm-obj-pos 0 0 + :offset (float-vector 0 0 (- (* i -30) 30))))))) + (when do-stop-grasp + (unless (eq pick-result :grasp-succeeded) + (send *ri* :stop-grasp :larm) + (send *ri* :stop-grasp :rarm))) + ; (send *ri* :move-hand :larm + ; (send *baxter* :hand-grasp-pre-pose :larm :opposed) 1000 :wait nil) + ; (send *ri* :move-hand :rarm + ; (send *baxter* :hand-grasp-pre-pose :rarm :opposed) 1000 :wait nil) + ; (send *ri* :wait-interpolation) + (send *ri* :gripper-servo-on :larm) + (send *ri* :gripper-servo-on :rarm) + pick-result)) + (:dualarm-try-to-pick-object + (larm-obj-pos rarm-obj-pos larm-suction-yaw rarm-suction-yaw &key (offset #f(0 0 0))) + (let (larm-graspingp rarm-grasingp av + (larm-coords-before-approach (send *baxter* :larm :end-coords :copy-worldcoords)) + (rarm-coords-before-approach (send *baxter* :rarm :end-coords :copy-worldcoords)) + larm-prismatic-angle rarm-prismatic-angle + larm-ik-success rarm-ik-success moveit-success previous-av) + (send *ri* :move-hand :larm + (send *baxter* :hand-grasp-pre-pose :larm :cylindrical) 1000 :wait nil) + (send *ri* :move-hand :rarm + (send *baxter* :hand-grasp-pre-pose :rarm :cylindrical) 1000 :wait t) + ; (send *ri* :move-hand :larm + ; (send *baxter* :hand-grasp-pose :larm :spherical :angle 90) 1000 :wait nil) + ; (send *ri* :move-hand :rarm + ; (send *baxter* :hand-grasp-pose :rarm :spherical :angle 90) 1000 :wait nil) + ; (send *ri* :wait-interpolation) + ; (send *ri* :move-hand :larm (send *baxter* :hand-grasp-pose :larm :cylindrical) 1000) + ; (send *ri* :move-hand :rarm (send *baxter* :hand-grasp-pose :rarm :cylindrical) 1000) + ; (send *ri* :wait-interpolation) + (send larm-coords-before-approach :locate (v+ larm-obj-pos #f(0 0 300)) :world) + (send rarm-coords-before-approach :locate (v+ rarm-obj-pos #f(0 0 300)) :world) + (ros::ros-info "[:dualarm-try-to-pick-object] larm-suction-yaw: ~a rarm-suction-yaw: ~a" larm-suction-yaw rarm-suction-yaw) + ;; start the vacuum gripper before approaching to the object + (setq previous-av (send *baxter* :angle-vector)) + (setq larm-ik-success + (send *baxter* :larm :inverse-kinematics + (make-coords :pos (v+ larm-obj-pos #f(0 0 150)) + :rpy (float-vector larm-suction-yaw 0 0)) + :use-gripper t + :rotation-axis t)) + (setq rarm-ik-success + (send *baxter* :rarm :inverse-kinematics + (make-coords :pos (v+ rarm-obj-pos #f(0 0 150)) + :rpy (float-vector rarm-suction-yaw 0 0)) + :use-gripper t + :rotation-axis t)) + (unless (and larm-ik-success rarm-ik-success) + (ros::ros-error "[:dualarm-try-to-pick-object] IK failed") + (send *baxter* :angle-vector previous-av) + (return-from :dualarm-try-to-pick-object :ik-failed)) + (setq moveit-success + (send *ri* :angle-vector (send *baxter* :angle-vector) 3000 nil 0)) + (unless moveit-success + (ros::ros-error "[:dualarm-try-to-pick-object] Moveit failed") + (send *baxter* :angle-vector previous-av) + (return-from :dualarm-try-to-pick-object :moveit-failed)) + (send *ri* :wait-interpolation) + + (ros::ros-info "[:dualarm-try-to-pick-object] start vacuum gripper") + (send *ri* :start-grasp :larm) + (send *ri* :start-grasp :rarm) + ;; suction: prismatic-based approach + (send *baxter* :slide-gripper :larm 120 :relative nil) + (send *baxter* :slide-gripper :rarm 120 :relative nil) + (setq larm-ik-success + (send *baxter* :larm :inverse-kinematics + (make-coords :pos (v+ larm-obj-pos offset) + :rpy (float-vector larm-suction-yaw 0 0)) + :use-gripper t + :rotation-axis t)) + (setq rarm-ik-success + (send *baxter* :rarm :inverse-kinematics + (make-coords :pos (v+ rarm-obj-pos offset) + :rpy (float-vector rarm-suction-yaw 0 0)) + :use-gripper t + :rotation-axis t)) + (unless (and larm-ik-success rarm-ik-success) + (ros::ros-error "[:dualarm-try-to-pick-object] IK failed") + (send *baxter* :angle-vector previous-av) + (return-from :dualarm-try-to-pick-object :ik-failed)) + (send *ri* :angle-vector-raw (send *baxter* :angle-vector) 3000 nil 0) + (send *ri* :wait-interpolation) + ; (unless moveit-success + ; (ros::ros-error "[:dualarm-try-to-pick-object] Moveit failed") + ; (send *baxter* :angle-vector previous-av) + ; (return-from :dualarm-try-to-pick-object :moveit-failed)) + + (setq larm-prismatic-angle (send *baxter* :larm :gripper-x :joint-angle)) + (setq rarm-prismatic-angle (send *baxter* :rarm :gripper-x :joint-angle)) + (send *baxter* :slide-gripper :larm 0 :relative nil) + (send *baxter* :slide-gripper :rarm 0 :relative nil) + (send *ri* :angle-vector-raw (send *baxter* :angle-vector) 3000 nil 0) + (send *ri* :wait-interpolation) ;; move down only the hand palm + + (send *baxter* :slide-gripper :larm larm-prismatic-angle :relative nil) + (send *baxter* :slide-gripper :rarm rarm-prismatic-angle :relative nil) + (send *ri* :angle-vector-raw (send *baxter* :angle-vector) 3000 nil 0) + ; FIXME: :wait-interpolation-until using :prismatic-loaded sometimes ends too fast, + ; so currently we only check :grasp (suction pressure). + (send self :wait-interpolation-until-grasp-dualarm :grasp) + + (send *baxter* :slide-gripper :larm 120 :relative nil) ;; maximum angle of prismatic joint + (send *baxter* :slide-gripper :rarm 120 :relative nil) ;; maximum angle of prismatic joint + (send *ri* :angle-vector-raw (send *baxter* :angle-vector) 3000 nil 0) + (send self :wait-interpolation-until-grasp-dualarm :prismatic :grasp) + + (setq larm-graspingp (send *ri* :graspingp :larm :suction)) + (setq rarm-graspingp (send *ri* :graspingp :rarm :suction)) + (ros::ros-info "[:dualarm-pick-object-in-tote] arm:~a graspingp: ~a" :larm larm-graspingp) + (ros::ros-info "[:dualarm-pick-object-in-tote] arm:~a graspingp: ~a" :rarm rarm-graspingp) + + (unless (and larm-graspingp rarm-grasingp) + (ros::ros-info "[:dualarm-pick-object-in-tote] again approach to the object") + (let ((temp-av (send *baxter* :angle-vector))) + ;; only if robot can solve IK + (if (and (send *baxter* :larm :move-end-pos #f(0 0 -150) :local) + (send *baxter* :rarm :move-end-pos #f(0 0 -150) :local)) + (send *ri* :angle-vector-raw (send *baxter* :angle-vector) 3000 nil 0)) + (send self :wait-interpolation-until-grasp-dualarm :prismatic :grasp) + (send *ri* :angle-vector-raw (send *baxter* :angle-vector temp-av) 3000 nil 0) + (send self :wait-interpolation-until-grasp-dualarm :prismatic :grasp))) + ; ;; Open fingers in bin + ; (send *ri* :move-hand :larm + ; (send *baxter* :hand-grasp-pre-pose :larm :spherical) 1000 :wait nil) + ; (send *ri* :move-hand :rarm + ; (send *baxter* :hand-grasp-pre-pose :rarm :spherical) 1000 :wait nil) + ; (send *ri* :wait-interpolation) + + (send *baxter* :angle-vector (send *ri* :state :potentio-vector :wait-until-update t)) + (send *baxter* :rotate-gripper :larm 30 :relative nil) + (send *baxter* :rotate-gripper :rarm 30 :relative nil) + (send *ri* :angle-vector-raw (send *baxter* :angle-vector) 1000 nil 0) + (send *ri* :wait-interpolation) + + ;; suction: prismatic-based approach + (send *baxter* :slide-gripper :larm 50 :relative nil) + (send *baxter* :slide-gripper :rarm 50 :relative nil) + (send *ri* :angle-vector-raw (send *baxter* :angle-vector) 3000 nil 0) + ;; suction: prismatic-based approach + ;; lift object + (ros::ros-info "[:dualarm-pick-object-in-tote] lift the object") + (send *ri* :gripper-servo-off :larm) + (send *ri* :gripper-servo-off :rarm) + + (let ((larm-tc (send *baxter* :larm :end-coords :copy-worldcoords)) + (rarm-tc (send *baxter* :rarm :end-coords :copy-worldcoords))) + ;; overwrite only world-z + (setf (aref (send larm-tc :worldpos) 2) + (elt (send larm-coords-before-approach :worldpos) 2)) + (setf (aref (send rarm-tc :worldpos) 2) + (elt (send rarm-coords-before-approach :worldpos) 2)) + (send *baxter* :larm :inverse-kinematics larm-tc :rotation-axis t) + (send *baxter* :rarm :inverse-kinematics rarm-tc :rotation-axis t)) + (send *ri* :angle-vector-raw (send *baxter* :angle-vector) 3000 nil 0) + (send *ri* :wait-interpolation) + + (setq graspingp (and (send *ri* :graspingp :larm :suction) + (send *ri* :graspingp :rarm :suction))) + (ros::ros-info "[:dualarm-pick-object-in-tote] graspingp: ~a" graspingp) + (if graspingp :grasp-succeeded :grasp-failed))) + (:return-from-recognize-object (arm) + (let ((arms (if (eq arm :arms) (list :larm :rarm) (list arm)))) + (dolist (tmp-arm arms) + (ros::ros-info "[:return-from-recognize-object] ~A: return to arc-reset-pose" tmp-arm) + (send *ri* :angle-vector (send *baxter* :arc-reset-pose tmp-arm) 5000 nil 0) + (send *ri* :wait-interpolation)))) + (:return-from-pick-object (arm) + (let ((arms (if (eq arm :arms) (list :larm :rarm) (list arm)))) + (dolist (tmp-arm arms) + (ros::ros-info "[:return-from-pick-object] ~A: return to arc-reset-pose" tmp-arm) + (send *ri* :angle-vector (send *baxter* :arc-reset-pose tmp-arm) 5000 nil 0) + (send *ri* :wait-interpolation)))) + (:verify-object () + (let ((stamp (ros::time-now))) + (if use-scale- + (progn + (when (eq scale-candidates- :timeout) + (ros::ros-error "subscribed weight candidates, but timeout") + (return-from :verify-object nil)) + (ros::ros-info-green "[main] arm ~a: scale candidates ~a" arm scale-candidates-) + (unless scale-candidates- (return-from :verify-object nil)) + (find target-obj- scale-candidates- :test #'string=) + (if target-obj- t nil)) + t))) + (:place-object (arm &key (opposite nil) (distance 400)) + (send *ri* :speak + (format nil "placing ~a in target tote." (underscore-to-space target-obj-))) + (ros::ros-info "[main] place ~a in target tote" (underscore-to-space target-obj-)) + (when moveit-p- (send self :delete-target-tote-scene)) + (if (eq arm :arms) + (progn + (send *baxter* :larm :move-end-pos + (float-vector (* (if opposite -1 1) distance) 0 0) + :world :rotation-axis t :use-gripper t) + (send *baxter* :rarm :move-end-pos + (float-vector (* (if opposite -1 1) distance) 0 0) + :world :rotation-axis t :use-gripper t)) + (progn + (send *baxter* arm :inverse-kinematics + (make-coords :pos (send (send *baxter* :larm :end-coords :copy-worldcoords) :worldpos) + :rpy (float-vector (if (eq arm :larm) -90 90) 0 0)) + :use-gripper t + :rotation-axis t) + (send *baxter* arm :move-end-pos + (float-vector (* (if opposite -1 1) distance) 0 0) + :world :rotation-axis :z :use-gripper t))) + (send *ri* :angle-vector-raw (send *baxter* :angle-vector) 3000 nil 0) + (send *ri* :wait-interpolation) + (if (eq arm :arms) + (progn + (send *baxter* :larm :move-end-pos #f(0 0 -150) :world :rotation-axis t :use-gripper t) + (send *baxter* :rarm :move-end-pos #f(0 0 -150) :world :rotation-axis t :use-gripper t)) + (send *baxter* arm :move-end-pos #f(0 0 -150) :world :rotation-axis :z :use-gripper t)) + (send *ri* :angle-vector-raw (send *baxter* :angle-vector) 3000 nil 0) + (send *ri* :wait-interpolation) + (send *ri* :stop-grasp :arms) + (if (eq arm :arms) + (progn + (send self :spin-off-by-wrist :larm :times 3) + (send self :spin-off-by-wrist :rarm :times 3)) + (send self :spin-off-by-wrist arm :times 3)) + (send self :spin-off-by-wrist arm :times 3) + (send *ri* :wait-interpolation) + (ros::ros-info "[main] ~a, place object in bin" arm) + (if (eq arm :arms) + (progn + (send *baxter* :larm :move-end-pos #f(0 0 150) :world :rotation-axis t :use-gripper t) + (send *baxter* :rarm :move-end-pos #f(0 0 150) :world :rotation-axis t :use-gripper t)) + (send *baxter* arm :move-end-pos #f(0 0 150) :world :rotation-axis :z :use-gripper t)) + (send *ri* :angle-vector-raw (send *baxter* :angle-vector) 3000 nil 0) + (send *ri* :wait-interpolation) + (when moveit-p- (send self :add-target-tote-scene)) + (send *ri* :angle-vector (send *baxter* :arc-reset-pose :arms) 3000 nil 0) + (send *ri* :wait-interpolation)) + (:return-from-place-object (arm) + (if (eq arm :arms) + (progn + (send-super :return-from-place-object :larm) + (send-super :return-from-place-object :rarm)) + (send-super :return-from-place-object arm))) + (:return-object (arm) + (send *ri* :speak "returning object") + (ros::ros-info "[main] return in tote") + (when moveit-p- (send self :delete-tote-scene)) + (if (eq arm :arms) + (progn + (send *baxter* :larm :move-end-pos #f(0 0 -300) :world :rotation-axis t) + (send *baxter* :rarm :move-end-pos #f(0 0 -300) :world :rotation-axis t)) + (send *baxter* arm :move-end-pos #f(0 0 -300) :world :rotation-axis :z)) + (send *ri* :angle-vector-raw (send *baxter* :angle-vector) 3000 nil 0) + (send *ri* :wait-interpolation) + (send *ri* :stop-grasp arm) + (if (eq arm :arms) + (progn + (send self :spin-off-by-wrist :larm :times 3) + (send self :spin-off-by-wrist :rarm :times 3)) + (send self :spin-off-by-wrist arm :times 3)) + (send *ri* :wait-interpolation) + (ros::ros-info "[main] ~a, return object in tote" arm) + (if (eq arm :arms) + (progn + (send *baxter* :larm :move-end-pos #f(0 0 300) :world :rotation-axis :z) + (send *baxter* :rarm :move-end-pos #f(0 0 300) :world :rotation-axis :z)) + (send *baxter* arm :move-end-pos #f(0 0 300) :world :rotation-axis :z)) + (send *ri* :angle-vector-raw (send *baxter* :angle-vector) 3000 nil 0) + (send *ri* :wait-interpolation) + (when moveit-p- (send self :add-tote-scene)) + (send *ri* :angle-vector (send *baxter* :arc-reset-pose :arms) 3000 nil 0) + (send *ri* :wait-interpolation)) + (:calib-prismatic-joint (arm) + (if (eq arm :arms) + (progn + (send-super :calib-prismatic-joint :larm) + (send-super :calib-prismatic-joint :rarm)) + (send-super :calib-prismatic-joint arm))) + (:giveup () + (let ((srv-name (format nil "/~a_hand_camera/dualarm_grasp_segmentation/get_another" (arm2str recognize-arm-))) + (req (instance dualarm_grasping::GetAnotherRequest :init))) + (ros::wait-for-service srv-name) + (send req :style (if (eq grasping-way- :dual) "dual" "single")) + (send req :label target-label-) + (send (ros::service-call srv-name req) :success))) + (:reset-giveup (arm) + (let ((srv-name (format nil "/~a_hand_camera/dualarm_grasp_segmentation/reset" (arm2str arm))) + (req (instance std_srvs::TriggerRequest :init))) + (ros::wait-for-service srv-name) + (send (ros::service-call srv-name req) :success))) + (:set-grasping-way (grasping-way) + (let ((dynsrv-name + (format nil "/~a_hand_camera/dualarm_grasp_segmentation" + (arm2str recognize-arm-))) + (dynparam-name "grasping_way")) + (ros::set-dynparam dynsrv-name + (cons dynparam-name grasping-way)) + (setq grasping-way- + (if (equal grasping-way "single") :single :dual)) + (ros::ros-info "set grasping-way: ~A" grasping-way))) + (:recognition-save-request () + (let ((savesrv-name + (format nil "/~a_hand_camera/before_grasping/data_collection/save_request" + (arm2str recognize-arm-)))) + (ros::wait-for-service savesrv-name) + (if (send (ros::service-call savesrv-name (instance std_srvs::TriggerRequest :init)) :success) + (ros::ros-info "[:recognition-save-request] success to save recognition data") + (ros::ros-error "[:recognition-save-request] fail to save recognition data")))) + (:result-save-request () + (let ((savesrv-name + (format nil "/~a_hand_camera/after_grasping/data_collection/save_request" + (arm2str recognize-arm-)))) + (ros::wait-for-service savesrv-name) + (if (send (ros::service-call savesrv-name (instance std_srvs::TriggerRequest :init)) :success) + (ros::ros-info "[:result-save-request] success to save result data") + (ros::ros-error "[:result-save-request] fail to save result data")))) + (:set-save-dir () + (let* ((before-dynsrv-name + (format nil "/~a_hand_camera/before_grasping/data_collection/" + (arm2str recognize-arm-))) + (after-dynsrv-name + (format nil "/~a_hand_camera/after_grasping/data_collection/" + (arm2str recognize-arm-))) + (localtime (unix::localtime)) + (stamp (format nil "~A~0,2d~0,2d_~0,2d~0,2d~0,2d" + (+ (elt localtime 5) 1900) + (+ (elt localtime 4) 1) + (elt localtime 3) (elt localtime 2) + (elt localtime 1) (elt localtime 0))) + (before-save-dir (format nil "~~/.ros/dualarm_grasping/recognition/~A" stamp)) + (after-save-dir (format nil "~~/.ros/dualarm_grasping/result/~A" stamp))) + (ros::set-dynparam before-dynsrv-name (cons "save_dir" before-save-dir)) + (ros::set-dynparam after-dynsrv-name (cons "save_dir" after-save-dir)))) + (:set-result (pick-result) + (ros::set-param "/result" + (cond ((eq pick-result :grasp-succeeded) "success") + ((eq pick-result :grasp-unstable) "unstable_grasp") + ((eq pick-result :grasp-failed) "drop") + ((eq pick-result :ik-failed) "ik_failed") + ((eq pick-result :moveit-failed) "moveit_failed") + (t "unknown")))) + (:graspingp (arm) + (if (eq arm :arms) + (and (send *ri* :graspingp :larm :suction) + (send *ri* :graspingp :rarm :suction)) + (send *ri* :graspingp arm :suction))) + (:prismaticp (arm) + (let ((prismatic-load (send *ri* :get-prismatic-load arm))) + (and (< prismatic-load -0.07) (< (send *ri* :get-prismatic-vel arm) 0.01)))) + (:wait-interpolation-until-grasp-dualarm (&rest args) + (let ((arms (list :larm :rarm)) + suctionp prismaticp) + ;; wait for :interpolatingp + (setq suctionp (memq :suction args)) + (setq prismaticp (memq :prismatic args)) + (dotimes (x 100) + (if (send *ri* :interpolatingp) (return)) + (unix::usleep 1000)) + (while (send *ri* :interpolatingp) + (dolist (tmp-arm arms) + (when + (cond + ((null suctionp) (send self :prismaticp tmp-arm)) + ((null prismaticp) (send self :graspingp tmp-arm)) + (t (or (send self :graspingp tmp-arm) (send self :prismaticp tmp-arm)))) + (ros::ros-info "[:wait-interpolation-until-grasp-dualarm] Cancel angle vector: ~a" tmp-arm) + (send *ri* :cancel-angle-vector + :controller-type (send *ri* :get-arm-controller tmp-arm)) + (setq arms (remove tmp-arm arms)))) + (send *ri* :spin-once) + (unix::usleep 1000))))) + + +(defun dualarm_grasping::dualarm-grasp-init + (&key (ctype :default-controller) (moveit nil) (scale nil) (collision-avoidance nil)) + (let (mvit-env mvit-rb) + (when moveit + (setq mvit-env (instance jsk_arc2017_baxter::baxter-moveit-environment)) + (setq mvit-rb (instance jsk_arc2017_baxter::baxter-robot :init))) + (unless (boundp '*ri*) + (setq *ri* (instance jsk_arc2017_baxter::baxter-interface :init :type ctype + :moveit-environment mvit-env + :moveit-robot mvit-rb))) + (unless (boundp '*baxter*) + (if collision-avoidance + (setq *baxter* (instance jsk_arc2017_baxter::baxter-robot-safe :init)) + (setq *baxter* (instance jsk_arc2017_baxter::baxter-robot :init)))) + (unless (boundp '*co*) + (setq *co* (when moveit (instance collision-object-publisher :init)))) + (unless (boundp '*ti*) + (setq *ti* (instance dualarm_grasping::dualarm-grasp-interface :init + :moveit moveit :scale scale))) + (send *baxter* :angle-vector (send *ri* :state :potentio-vector)) + (send *ri* :calib-grasp :arms))) diff --git a/demos/selective_dualarm_grasping/euslisp/sampling-grasp.l b/demos/selective_dualarm_grasping/euslisp/sampling-grasp.l new file mode 100755 index 000000000..481e7032d --- /dev/null +++ b/demos/selective_dualarm_grasping/euslisp/sampling-grasp.l @@ -0,0 +1,123 @@ +#!/usr/bin/env roseus + +(ros::roseus "robot_main") +(require "package://dualarm_grasping/euslisp/lib/dualarm-grasp-interface.l") +;; smach +(require :state-machine "package://roseus_smach/src/state-machine.l") +(require :state-machine-ros "package://roseus_smach/src/state-machine-ros.l") +(require :state-machine-utils "package://roseus_smach/src/state-machine-utils.l") + +(make-random-state t) + + +(defun make-sampling-grasp-state-machine () + (setq *sm* + (make-state-machine + '((:init -> :recognize-bboxes) + (:recognize-bboxes -> :wait-for-user-input) + (:wait-for-user-input -> :recognize-object) + (:wait-for-user-input !-> :finish) + (:recognize-object -> :pick-object) + (:recognize-object !-> :recognize-object) + (:recognize-object :give-up :wait-for-user-input) + (:pick-object -> :return-object) + (:pick-object !-> :recognize-object) + (:return-object -> :recognize-object)) + '((:init + '(lambda (userdata) + (ros::ros-info-green "[sampling-grasp] start picking") + t)) + (:recognize-bboxes + '(lambda (userdata) + (send *ti* :recognize-bboxes) + t)) + (:wait-for-user-input + '(lambda (userdata) + (send *ti* :wait-for-user-input))) + (:recognize-object + '(lambda (userdata) + (let ((arm (elt '(:larm :rarm) (random 2))) + (grasping-way (cdr (assoc 'grasping-way userdata))) + success-p) + (setq arm (send *ti* :set-recognize-arm arm :update t)) + (send *ti* :set-grasping-way grasping-way) + (unix::sleep 1) + (setq success-p + (send *ti* :recognize-object arm)) + (if success-p + (progn + (send *ti* :set-fail-recognize-arm nil) + (send *ti* :set-save-dir) + (send *ti* :recognition-save-request)) + (progn + (send *ti* :set-fail-recognize-arm arm) + (send *ti* :return-from-recognize-object arm))) + (setf (cdr (assoc 'grasp-arm userdata)) + (send *ti* :get-grasp-arm arm)) + success-p))) + (:pick-object + '(lambda (userdata) + (let (pick-result success-p (arm (cdr (assoc 'grasp-arm userdata)))) + (send *ti* :calib-prismatic-joint arm) + (setq pick-result (send *ti* :pick-object arm)) + (unix::sleep 1) + (setq success-p (send *ti* :graspingp arm)) + (if success-p + (ros::ros-info-green "[sampling-grasp] pick succeeded") + (progn + (when (eq pick-result :grasp-succeeded) (setq pick-result :grasp-unstable)) + (ros::ros-error "[sampling-grasp] pick failed because of ~A" pick-result))) + (send *ti* :set-result pick-result) + (send *ti* :result-save-request) + (unless success-p + (send *ri* :stop-grasp arm) + (send *ti* :return-from-pick-object arm)) + success-p))) + (:return-object + '(lambda (userdata) + (let ((arm (cdr (assoc 'grasp-arm userdata)))) + (send *ti* :return-object arm) + (send *ti* :return-from-pick-object arm) + (send *ti* :calib-prismatic-joint arm)) + t))) + '(:init) + '(:finish)))) + + +(defun sampling-grasp-init (&key (ctype :default-controller) (calib-pressure t) + (moveit t) (scale nil)) + (dualarm_grasping::dualarm-grasp-init :ctype ctype :moveit moveit :scale scale) + (when moveit + (send *ti* :wipe-all-scene) + (send *ti* :add-workspace-scene)) + (send *ri* :gripper-servo-on) + ;; initialize fingers + (send *ri* :move-hand :rarm + (send *baxter* :hand-grasp-pre-pose :rarm :cylindrical) 1000 :wait nil) + (send *ri* :move-hand :larm + (send *baxter* :hand-grasp-pre-pose :larm :cylindrical) 1000) + (send *ri* :move-hand :rarm + (send *baxter* :hand-grasp-pre-pose :rarm :opposed) 1000 :wait nil) + (send *ri* :move-hand :larm + (send *baxter* :hand-grasp-pre-pose :larm :opposed) 1000) + (send *ri* :angle-vector (send *baxter* :arc-reset-pose)) + (send *ri* :wait-interpolation) + (when calib-pressure + (send *ri* :calib-pressure-threshold :rarm) + (send *ri* :calib-pressure-threshold :larm)) + (send *ti* :calib-prismatic-joint :arms) + (objects (list *baxter*)) + t) + + +(defun sampling-grasp-mainloop (gway) + (when (not (boundp '*sm*)) + (make-sampling-grasp-state-machine)) + (exec-state-machine *sm* `((grasp-arm . :larm) + (grasping-way . ,(if (eq gway :dual) "dual" "single"))) :hz 2.0)) + + +(warn "~% Commands ~%") +(warn "(sampling-grasp-init) : initialize *ti*~%") +(warn "(sampling-grasp-mainloop :single) : start single arm sampling mainloop~%") +(warn "(sampling-grasp-mainloop :dual) : start dual arm sampling mainloop~%") diff --git a/demos/selective_dualarm_grasping/euslisp/test-grasp.l b/demos/selective_dualarm_grasping/euslisp/test-grasp.l new file mode 100755 index 000000000..83c3652b2 --- /dev/null +++ b/demos/selective_dualarm_grasping/euslisp/test-grasp.l @@ -0,0 +1,119 @@ +#!/usr/bin/env roseus + +(ros::roseus "robot_main") +(require "package://dualarm_grasping/euslisp/lib/dualarm-grasp-interface.l") +;; smach +(require :state-machine "package://roseus_smach/src/state-machine.l") +(require :state-machine-ros "package://roseus_smach/src/state-machine-ros.l") +(require :state-machine-utils "package://roseus_smach/src/state-machine-utils.l") + +(make-random-state t) + + +(defun make-test-grasp-state-machine () + (setq *sm* + (make-state-machine + '((:init -> :recognize-bboxes) + (:recognize-bboxes -> :wait-for-user-input) + (:wait-for-user-input -> :recognize-object) + (:wait-for-user-input !-> :finish) + (:recognize-object -> :pick-object) + (:recognize-object !-> :recognize-object) + (:recognize-object :give-up :wait-for-user-input) + (:pick-object -> :return-object) + (:pick-object !-> :wait-for-user-input) + (:return-object -> :wait-for-user-input)) + '((:init + '(lambda (userdata) + (ros::ros-info-green "[test-grasp] start picking") + t)) + (:recognize-bboxes + '(lambda (userdata) + (send *ti* :recognize-bboxes) + t)) + (:wait-for-user-input + '(lambda (userdata) + (send *ti* :wait-for-user-input))) + (:recognize-object + '(lambda (userdata) + (let ((arm (elt '(:larm :rarm) (random 2))) + success-p) + (setq arm (send *ti* :set-recognize-arm arm :update t)) + (unix::sleep 1) + (setq success-p + (send *ti* :recognize-object arm)) + (if success-p + (progn + (send *ti* :set-fail-recognize-arm nil) + (send *ti* :set-save-dir) + (send *ti* :recognition-save-request)) + (progn + (send *ti* :set-fail-recognize-arm arm) + (send *ti* :return-from-recognize-object arm))) + (setf (cdr (assoc 'grasp-arm userdata)) + (send *ti* :get-grasp-arm arm)) + success-p))) + (:pick-object + '(lambda (userdata) + (let (pick-result success-p (arm (cdr (assoc 'grasp-arm userdata)))) + (send *ti* :calib-prismatic-joint arm) + (setq pick-result (send *ti* :pick-object arm)) + (unix::sleep 1) + (setq success-p (send *ti* :graspingp arm)) + (if success-p + (ros::ros-info-green "[test-grasp] pick succeeded") + (progn + (when (eq pick-result :grasp-succeeded) (setq pick-result :grasp-unstable)) + (ros::ros-error "[test-grasp] pick failed because of ~A" pick-result))) + (send *ti* :set-result pick-result) + (send *ti* :result-save-request) + (unless success-p + (send *ri* :stop-grasp arm) + (send *ti* :return-from-pick-object arm)) + success-p))) + (:return-object + '(lambda (userdata) + (let ((arm (cdr (assoc 'grasp-arm userdata)))) + (send *ti* :return-object arm) + (send *ti* :return-from-pick-object arm) + (send *ti* :calib-prismatic-joint arm)) + t))) + '(:init) + '(:finish)))) + + +(defun test-grasp-init (&key (ctype :default-controller) (calib-pressure t) + (moveit t) (scale nil)) + (dualarm_grasping::dualarm-grasp-init :ctype ctype :moveit moveit :scale scale) + (when moveit + (send *ti* :wipe-all-scene) + (send *ti* :add-workspace-scene)) + (send *ri* :gripper-servo-on) + ;; initialize fingers + (send *ri* :move-hand :rarm + (send *baxter* :hand-grasp-pre-pose :rarm :cylindrical) 1000 :wait nil) + (send *ri* :move-hand :larm + (send *baxter* :hand-grasp-pre-pose :larm :cylindrical) 1000) + (send *ri* :move-hand :rarm + (send *baxter* :hand-grasp-pre-pose :rarm :opposed) 1000 :wait nil) + (send *ri* :move-hand :larm + (send *baxter* :hand-grasp-pre-pose :larm :opposed) 1000) + (send *ri* :angle-vector (send *baxter* :arc-reset-pose)) + (send *ri* :wait-interpolation) + (when calib-pressure + (send *ri* :calib-pressure-threshold :rarm) + (send *ri* :calib-pressure-threshold :larm)) + (send *ti* :calib-prismatic-joint :arms) + (objects (list *baxter*)) + t) + + +(defun test-grasp-mainloop () + (when (not (boundp '*sm*)) + (make-test-grasp-state-machine)) + (exec-state-machine *sm* `((grasp-arm . :larm)) :hz 2.0)) + + +(warn "~% Commands ~%") +(warn "(test-grasp-init) : initialize *ti*~%") +(warn "(test-grasp-mainloop) : start test mainloop~%") diff --git a/demos/selective_dualarm_grasping/launch/setup/baxter.launch b/demos/selective_dualarm_grasping/launch/setup/baxter.launch new file mode 100644 index 000000000..9dd175a77 --- /dev/null +++ b/demos/selective_dualarm_grasping/launch/setup/baxter.launch @@ -0,0 +1,99 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + file_name: $(find jsk_apc2016_common)/resource/wallpapers/wallpaper_black.png + + + + + + + + + + + + + + + + + + diff --git a/demos/selective_dualarm_grasping/launch/setup/include/baxter/recognition.launch b/demos/selective_dualarm_grasping/launch/setup/include/baxter/recognition.launch new file mode 100644 index 000000000..0e157dbca --- /dev/null +++ b/demos/selective_dualarm_grasping/launch/setup/include/baxter/recognition.launch @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/demos/selective_dualarm_grasping/launch/setup/include/common/dualarm_grasp_segmentation.launch b/demos/selective_dualarm_grasping/launch/setup/include/common/dualarm_grasp_segmentation.launch new file mode 100644 index 000000000..2f2625db9 --- /dev/null +++ b/demos/selective_dualarm_grasping/launch/setup/include/common/dualarm_grasp_segmentation.launch @@ -0,0 +1,178 @@ + + + + + + + + + + + + + + + + + + + + + + + gpu: $(arg GPU) + model_file: $(arg MODEL_FILE) + score_thresh: 0.5 + grasp_thresh: 0.2 + use_mask: true + queue_size: 100 + approximate_sync: true + sampling: $(arg SAMPLING) + sampling_weighted: true + sampling_thresh: 0.7 + + + + + + + + + gpu: $(arg GPU) + model_file: $(arg MODEL_FILE) + score_thresh: 0.5 + grasp_thresh: 0.2 + use_mask: false + queue_size: 100 + approximate_sync: true + sampling: $(arg SAMPLING) + sampling_weighted: true + sampling_thresh: 0.7 + + + + + + + + + + + + + approximate_sync: true + queue_size: 100 + keep_organized: true + + + + + + + min_size: 10 + max_size: 10000 + tolerance: 0.02 + + + + + + + + approximate_sync: false + queue_size: 100 + sort_by: -cloud_size + align_boxes: true + align_boxes_with_plane: false + use_pca: $(arg USE_PCA) + target_frame_id: $(arg FIXED_FRAME) + + + + + + + + + + + + + save_dir: ~/.ros/dualarm_grasping/recognition + timestamp_save_dir: false + slop: 5.0 + topics: + - name: $(arg INPUT_IMAGE) + msg_class: sensor_msgs/Image + fname: input_image.png + savetype: ColorImage + - name: $(arg CAMERA_NAMESPACE)/$(arg INPUT_MASK) + msg_class: sensor_msgs/Image + fname: input_mask.png + savetype: ColorImage + - name: $(arg CAMERA_NAMESPACE)/dualarm_grasp_segmentation/debug/net_input + msg_class: sensor_msgs/Image + fname: net_input.png + savetype: ColorImage + - name: $(arg CAMERA_NAMESPACE)/dualarm_grasp_segmentation/output/label + msg_class: sensor_msgs/Image + fname: label.png + savetype: LabelImage + - name: $(arg CAMERA_NAMESPACE)/dualarm_grasp_segmentation/output/grasp_mask + msg_class: sensor_msgs/Image + fname: sampled_grasp_mask.png + savetype: ColorImage + - name: $(arg CAMERA_NAMESPACE)/dualarm_grasp_segmentation/output/single/mask + msg_class: sensor_msgs/Image + fname: single_grasp_mask.png + savetype: ColorImage + - name: $(arg CAMERA_NAMESPACE)/dualarm_grasp_segmentation/output/dual/mask + msg_class: sensor_msgs/Image + fname: dual_grasp_mask.png + savetype: ColorImage + - name: $(arg CAMERA_NAMESPACE)/dualarm_grasp_segmentation/output/grasp_class + msg_class: dualarm_grasping/GraspClassificationResult + fname: grasp_class.yaml + savetype: YAML + + + + + + + + save_dir: ~/.ros/dualarm_grasping/result + timestamp_save_dir: false + slop: 2.0 + params: + - key: /result + fname: result.txt + savetype: Text + - key: /tote_contents + fname: tote_contents.txt + savetype: YAML + - key: $(arg CAMERA_NAMESPACE)/dualarm_grasp_segmentation/grasping_way + fname: grasping_way.txt + savetype: Text + + + + + diff --git a/demos/selective_dualarm_grasping/launch/setup/include/common/dualarm_occluded_grasp_instance_segmentation.launch b/demos/selective_dualarm_grasping/launch/setup/include/common/dualarm_occluded_grasp_instance_segmentation.launch new file mode 100644 index 000000000..b59950c9f --- /dev/null +++ b/demos/selective_dualarm_grasping/launch/setup/include/common/dualarm_occluded_grasp_instance_segmentation.launch @@ -0,0 +1,292 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + gpu: $(arg GPU) + model_file: $(arg MODEL_FILE) + use_mask: true + queue_size: 100 + approximate_sync: true + score_thresh: 0.5 + grasp_thresh: 0.3 + nms_thresh: 0.3 + vis_thresh: 0.8 + sampling: $(arg SAMPLING) + sampling_thresh: 0.7 + sampling_weighted: true + config_yaml: $(arg CONFIG_YAML) + target_grasp: $(arg TARGET_GRASP) + + + + + + + + + + gpu: $(arg GPU) + model_file: $(arg MODEL_FILE) + use_mask: false + queue_size: 100 + approximate_sync: true + score_thresh: 0.5 + grasp_thresh: 0.3 + nms_thresh: 0.3 + vis_thresh: 0.8 + sampling: $(arg SAMPLING) + sampling_thresh: 0.7 + sampling_weighted: true + config_yaml: $(arg CONFIG_YAML) + target_grasp: $(arg TARGET_GRASP) + + + + + + + + + + + + + + approximate_sync: true + queue_size: 100 + keep_organized: true + + + + + + + min_size: 10 + max_size: 10000 + tolerance: 0.02 + + + + + + + + approximate_sync: false + queue_size: 100 + sort_by: -cloud_size + align_boxes: true + align_boxes_with_plane: false + use_pca: $(arg USE_PCA) + target_frame_id: $(arg FIXED_FRAME) + + + + + + + save_dir: ~/.ros/dualarm_grasping/recognition + timestamp_save_dir: false + slop: 5.0 + topics: + - name: $(arg INPUT_IMAGE) + msg_class: sensor_msgs/Image + fname: input_image.png + savetype: ColorImage + - name: $(arg CAMERA_NAMESPACE)/$(arg INPUT_MASK) + msg_class: sensor_msgs/Image + fname: input_mask.png + savetype: ColorImage + - name: $(arg CAMERA_NAMESPACE)/dualarm_grasp_segmentation/debug/net_input + msg_class: sensor_msgs/Image + fname: net_input.png + savetype: ColorImage + - name: $(arg CAMERA_NAMESPACE)/dualarm_grasp_segmentation/output/vis/labels + msg_class: jsk_recognition_msgs/LabelArray + fname: vis_labels.yaml + savetype: YAML + - name: $(arg CAMERA_NAMESPACE)/dualarm_grasp_segmentation/output/vis/cls_label + msg_class: sensor_msgs/Image + fname: vis_cls_label.npz + savetype: LabelImage + - name: $(arg CAMERA_NAMESPACE)/dualarm_grasp_segmentation/output/vis/ins_label + msg_class: sensor_msgs/Image + fname: vis_ins_label.npz + savetype: LabelImage + - name: $(arg CAMERA_NAMESPACE)/dualarm_grasp_segmentation/output/occ/labels + msg_class: jsk_recognition_msgs/LabelArray + fname: occ_labels.yaml + savetype: YAML + - name: $(arg CAMERA_NAMESPACE)/dualarm_grasp_segmentation/output/occ/cls_label + msg_class: sensor_msgs/Image + fname: occ_cls_label.npz + savetype: LabelImage + - name: $(arg CAMERA_NAMESPACE)/dualarm_grasp_segmentation/output/occ/ins_label + msg_class: sensor_msgs/Image + fname: occ_ins_label.npz + savetype: LabelImage + - name: $(arg CAMERA_NAMESPACE)/dualarm_grasp_segmentation/output/single/labels + msg_class: jsk_recognition_msgs/LabelArray + fname: single_labels.yaml + savetype: YAML + - name: $(arg CAMERA_NAMESPACE)/dualarm_grasp_segmentation/output/single/cls_label + msg_class: sensor_msgs/Image + fname: single_cls_label.npz + savetype: LabelImage + - name: $(arg CAMERA_NAMESPACE)/dualarm_grasp_segmentation/output/single/ins_label + msg_class: sensor_msgs/Image + fname: single_ins_label.npz + savetype: LabelImage + - name: $(arg CAMERA_NAMESPACE)/dualarm_grasp_segmentation/output/dual/labels + msg_class: jsk_recognition_msgs/LabelArray + fname: dual_labels.yaml + savetype: YAML + - name: $(arg CAMERA_NAMESPACE)/dualarm_grasp_segmentation/output/dual/cls_label + msg_class: sensor_msgs/Image + fname: dual_cls_label.npz + savetype: LabelImage + - name: $(arg CAMERA_NAMESPACE)/dualarm_grasp_segmentation/output/dual/ins_label + msg_class: sensor_msgs/Image + fname: dual_ins_label.npz + savetype: LabelImage + - name: $(arg CAMERA_NAMESPACE)/dualarm_grasp_segmentation/output/rects + msg_class: jsk_recognition_msgs/RectArray + fname: rects.yaml + savetype: YAML + - name: $(arg CAMERA_NAMESPACE)/dualarm_grasp_segmentation/output/class + msg_class: jsk_recognition_msgs/ClassificationResult + fname: class.yaml + savetype: YAML + - name: $(arg CAMERA_NAMESPACE)/dualarm_grasp_segmentation/output/grasp_mask + msg_class: sensor_msgs/Image + fname: grasp_mask.png + savetype: ColorImage + - name: $(arg CAMERA_NAMESPACE)/dualarm_grasp_segmentation/output/grasp_class + msg_class: dualarm_grasping/GraspClassificationResult + fname: grasp_class.yaml + savetype: YAML + + + + + + + + save_dir: ~/.ros/dualarm_grasping/result + timestamp_save_dir: false + slop: 2.0 + params: + - key: /result + fname: result.txt + savetype: Text + - key: /tote_contents + fname: tote_contents.txt + savetype: YAML + - key: $(arg CAMERA_NAMESPACE)/dualarm_grasp_segmentation/grasping_way + fname: grasping_way.txt + savetype: Text + + + + + + + + + + approximate_sync: true + queue_size: 200 + + + + + + + approximate_sync: true + queue_size: 200 + + + + + + + approximate_sync: true + queue_size: 200 + + + + + + + approximate_sync: true + queue_size: 200 + + + + + input_topics: + - $(arg INPUT_IMAGE) + - image_cluster_indices_decomposer_vis/output + - image_cluster_indices_decomposer_occ/output + - image_cluster_indices_decomposer_single/output + - image_cluster_indices_decomposer_dual/output + - dualarm_grasp_segmentation/output/grasp_mask + draw_topic_name: true + font_scale: 0.6 + font_thickness: 1 + approximate_sync: true + no_sync: true + + + + diff --git a/demos/selective_dualarm_grasping/launch/setup/include/common/recognition.launch b/demos/selective_dualarm_grasping/launch/setup/include/common/recognition.launch new file mode 100644 index 000000000..6036704b0 --- /dev/null +++ b/demos/selective_dualarm_grasping/launch/setup/include/common/recognition.launch @@ -0,0 +1,91 @@ + + + + + + + + + + + + + + + + + + + + index: 0 + + + + + + + use_multiple_attention: false + + + + + + + keep_organized: true + approximate_sync: true + max_queue_size: 100 + + + + + + + + approximate_sync: true + queue_size: 100 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/demos/selective_dualarm_grasping/launch/setup/include/pr2/kinect_head_remote.launch b/demos/selective_dualarm_grasping/launch/setup/include/pr2/kinect_head_remote.launch new file mode 100644 index 000000000..f2fe0bd3a --- /dev/null +++ b/demos/selective_dualarm_grasping/launch/setup/include/pr2/kinect_head_remote.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/demos/selective_dualarm_grasping/launch/setup/include/pr2/recognition.launch b/demos/selective_dualarm_grasping/launch/setup/include/pr2/recognition.launch new file mode 100644 index 000000000..7c8ef7f5d --- /dev/null +++ b/demos/selective_dualarm_grasping/launch/setup/include/pr2/recognition.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/demos/selective_dualarm_grasping/launch/setup/setup_occluded_picking.launch b/demos/selective_dualarm_grasping/launch/setup/setup_occluded_picking.launch new file mode 100644 index 000000000..14b28bb40 --- /dev/null +++ b/demos/selective_dualarm_grasping/launch/setup/setup_occluded_picking.launch @@ -0,0 +1,127 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + display_interactive_manipulator: true + display_interactive_manipulator_only_selected: true + display_description_only_selected: true + + + + + + + config_file: $(arg TOTE_YAML) + config_auto_save: true + + + + + + + config_file: $(arg TARGET_TOTE_YAML) + config_auto_save: true + + + + + + + location: tote + json_dir: $(arg json_dir) + + + + + + + + + + + + json_dir: $(arg json_dir) + target_location: tote + + + + + + + + + json_dir: $(arg json_dir) + target_location: tote + + + + + + + + + json_dir: $(arg json_dir) + target_location: tote + + + + + + + + + + + + + + + + + + + + diff --git a/demos/selective_dualarm_grasping/launch/setup/setup_occluded_sampling.launch b/demos/selective_dualarm_grasping/launch/setup/setup_occluded_sampling.launch new file mode 100644 index 000000000..f54933138 --- /dev/null +++ b/demos/selective_dualarm_grasping/launch/setup/setup_occluded_sampling.launch @@ -0,0 +1,100 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + display_interactive_manipulator: true + display_interactive_manipulator_only_selected: true + display_description_only_selected: true + + + + + + + config_file: $(arg TOTE_YAML) + config_auto_save: true + + + + + + + config_file: $(arg TARGET_TOTE_YAML) + config_auto_save: true + + + + + tote_contents: + - $(arg item) + + + + + + + + + + + + + + + + + + + + + + diff --git a/demos/selective_dualarm_grasping/launch/setup/setup_occluded_test.launch b/demos/selective_dualarm_grasping/launch/setup/setup_occluded_test.launch new file mode 100644 index 000000000..baefe94a3 --- /dev/null +++ b/demos/selective_dualarm_grasping/launch/setup/setup_occluded_test.launch @@ -0,0 +1,100 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + display_interactive_manipulator: true + display_interactive_manipulator_only_selected: true + display_description_only_selected: true + + + + + + + config_file: $(arg TOTE_YAML) + config_auto_save: true + + + + + + + config_file: $(arg TARGET_TOTE_YAML) + config_auto_save: true + + + + + tote_contents: + - $(arg item) + + + + + + + + + + + + + + + + + + + + + + diff --git a/demos/selective_dualarm_grasping/launch/setup/setup_picking.launch b/demos/selective_dualarm_grasping/launch/setup/setup_picking.launch new file mode 100644 index 000000000..23aa2c362 --- /dev/null +++ b/demos/selective_dualarm_grasping/launch/setup/setup_picking.launch @@ -0,0 +1,112 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + display_interactive_manipulator: true + display_interactive_manipulator_only_selected: true + display_description_only_selected: true + + + + + + + config_file: $(find dualarm_grasping)/config/$(arg ROBOT)/tote_marker.yaml + config_auto_save: true + + + + + + + config_file: $(find dualarm_grasping)/config/$(arg ROBOT)/target_tote_marker.yaml + config_auto_save: true + + + + + + + location: tote + json_dir: $(arg json_dir) + + + + + + + + + + + + json_dir: $(arg json_dir) + target_location: tote + + + + + + + + + json_dir: $(arg json_dir) + target_location: tote + + + + + + + + + json_dir: $(arg json_dir) + target_location: tote + + + + + + + + + + + + + + + + diff --git a/demos/selective_dualarm_grasping/launch/setup/setup_sampling.launch b/demos/selective_dualarm_grasping/launch/setup/setup_sampling.launch new file mode 100644 index 000000000..7d2433755 --- /dev/null +++ b/demos/selective_dualarm_grasping/launch/setup/setup_sampling.launch @@ -0,0 +1,80 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + display_interactive_manipulator: true + display_interactive_manipulator_only_selected: true + display_description_only_selected: true + + + + + + + config_file: $(find dualarm_grasping)/config/$(arg ROBOT)/tote_marker.yaml + config_auto_save: true + + + + + + + config_file: $(find dualarm_grasping)/config/$(arg ROBOT)/target_tote_marker.yaml + config_auto_save: true + + + + + + + + + tote_contents: + - $(arg item) + + + + + + + + + + + + + + diff --git a/demos/selective_dualarm_grasping/launch/setup/setup_test.launch b/demos/selective_dualarm_grasping/launch/setup/setup_test.launch new file mode 100644 index 000000000..f0b1badc8 --- /dev/null +++ b/demos/selective_dualarm_grasping/launch/setup/setup_test.launch @@ -0,0 +1,80 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + display_interactive_manipulator: true + display_interactive_manipulator_only_selected: true + display_description_only_selected: true + + + + + + + config_file: $(find dualarm_grasping)/config/$(arg ROBOT)/tote_marker.yaml + config_auto_save: true + + + + + + + config_file: $(find dualarm_grasping)/config/$(arg ROBOT)/target_tote_marker.yaml + config_auto_save: true + + + + + + + + + tote_contents: + - $(arg item) + + + + + + + + + + + + + + diff --git a/demos/selective_dualarm_grasping/launch/utils/record_occlusion_dataset.launch b/demos/selective_dualarm_grasping/launch/utils/record_occlusion_dataset.launch new file mode 100644 index 000000000..2e5d6084c --- /dev/null +++ b/demos/selective_dualarm_grasping/launch/utils/record_occlusion_dataset.launch @@ -0,0 +1,114 @@ + + + + + + + + + + + + + + display_interactive_manipulator: true + display_interactive_manipulator_only_selected: true + display_description_only_selected: true + + + + + + config_file: $(arg TOTE_YAML) + config_auto_save: true + + + + + + + index: 0 + + + + + + + use_multiple_attention: false + + + + + + + keep_organized: true + approximate_sync: true + max_queue_size: 100 + + + + + + + approximate_sync: true + queue_size: 100 + + + + + + approximate_sync: true + queue_size: 100 + + + + + + + + clip: false + approximate_sync: true + queue_size: 100 + + + + + + save_dir: ~/.ros/dualarm_grasping/occlusion_dataset/00 + timestamp_save_dir: true + slop: 3.0 + topics: + - name: /right_hand_camera/left/rgb/image_rect_color + msg_class: sensor_msgs/Image + fname: rgb.png + savetype: ColorImage + - name: /convex_mask/output + msg_class: sensor_msgs/Image + fname: mask.png + savetype: ColorImage + - name: /apply_mask/output + msg_class: sensor_msgs/Image + fname: masked_rgb.png + savetype: ColorImage + + + + + + diff --git a/demos/selective_dualarm_grasping/msg/GraspClassificationResult.msg b/demos/selective_dualarm_grasping/msg/GraspClassificationResult.msg new file mode 100644 index 000000000..118bc17a7 --- /dev/null +++ b/demos/selective_dualarm_grasping/msg/GraspClassificationResult.msg @@ -0,0 +1,4 @@ +Header header +string style +bool is_target +jsk_recognition_msgs/ClassificationResult classification diff --git a/demos/selective_dualarm_grasping/node_scripts/dualarm_grasp_segmentation.py b/demos/selective_dualarm_grasping/node_scripts/dualarm_grasp_segmentation.py new file mode 100755 index 000000000..9fb915c92 --- /dev/null +++ b/demos/selective_dualarm_grasping/node_scripts/dualarm_grasp_segmentation.py @@ -0,0 +1,459 @@ +#!/usr/bin/env python + +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + +import chainer +import chainer.serializers as S +from chainer import Variable +import cupy +import cv2 +import numpy as np +import os.path as osp +import scipy +from sklearn.cluster import KMeans +import time +import yaml + +import cv_bridge +from dynamic_reconfigure.server import Server +from jsk_recognition_msgs.msg import ClassificationResult +from jsk_topic_tools import ConnectionBasedTransport +from jsk_topic_tools.log_utils import logerr_throttle +import message_filters +import rospy +from sensor_msgs.msg import Image +from std_srvs.srv import Trigger +from std_srvs.srv import TriggerResponse + +from dualarm_grasping.cfg import DualarmGraspSegmentationConfig +from dualarm_grasping.models import DualarmGraspFCN32s +from dualarm_grasping.msg import GraspClassificationResult +from dualarm_grasping.srv import GetAnother +from dualarm_grasping.srv import GetAnotherResponse + + +filepath = osp.dirname(osp.realpath(__file__)) + + +class DualarmGraspSegmentation(ConnectionBasedTransport): + + def __init__(self): + super(self.__class__, self).__init__() + self.gpu = rospy.get_param('~gpu', -1) + model_file = rospy.get_param('~model_file') + self.mean_bgr = np.array([104.00698793, 116.66876762, 122.67891434]) + + self.label_names = rospy.get_param('~label_names') + self.bg_index = rospy.get_param('~bg_index', 0) + self.sampling = rospy.get_param('~sampling', False) + self.sampling_weighted = rospy.get_param('~sampling_weighted', False) + cfgpath = rospy.get_param( + '~config_yaml', osp.join(filepath, '../yaml/config.yaml')) + with open(cfgpath, 'r') as f: + config = yaml.load(f) + + tote_contents = rospy.get_param('~tote_contents', None) + if tote_contents is None: + self.candidates = range(len(self.label_names)) + else: + self.candidates = [0] + [self.label_names.index(x) + for x in tote_contents] + + self.giveup_labels = { + 'single': [self.bg_index], + 'dual': [self.bg_index], + } + + self.model = DualarmGraspFCN32s( + n_class=len(self.label_names)) + S.load_npz(model_file, self.model) + + if 'alpha' in config: + alpha = config['alpha'] + if isinstance(alpha, dict): + self.model.alpha_single = alpha['single'] + self.model.alpha_dual = alpha['dual'] + else: + self.model.alpha_single = alpha + self.model.alpha_dual = 1.0 + else: + self.model.alpha_single = 1.0 + self.model.alpha_dual = 1.0 + + if 'frequency_balancing' in config: + frq_balancing = config['frequency_balancing'] + self.model.frq_balancing = frq_balancing + else: + self.model.frq_balancing = False + + chainer.global_config.train = False + chainer.global_config.enable_backprop = False + + if self.gpu != -1: + chainer.cuda.get_device(self.gpu).use() + self.model.to_gpu(self.gpu) + self.pub_input = self.advertise( + '~debug/net_input', Image, queue_size=1) + self.pub_seg = self.advertise( + '~output/seg_prob', Image, queue_size=1) + self.pub_label = self.advertise( + '~output/label', Image, queue_size=1) + + self.pub_sg_grasp = self.advertise( + '~output/single/grasp', Image, queue_size=1) + self.pub_sg_mask = self.advertise( + '~output/single/mask', Image, queue_size=1) + self.pub_sg_label = self.advertise( + '~output/single/class', ClassificationResult, queue_size=1) + + self.pub_dg_grasp = self.advertise( + '~output/dual/grasp', Image, queue_size=1) + self.pub_dg_mask = self.advertise( + '~output/dual/mask', Image, queue_size=1) + self.pub_dg_label = self.advertise( + '~output/dual/class', ClassificationResult, queue_size=1) + + self.pub_mask = self.advertise( + '~output/grasp_mask', Image, queue_size=1) + self.pub_grasp = self.advertise( + '~output/grasp_class', GraspClassificationResult, queue_size=1) + + self.get_another_service = rospy.Service( + '~get_another', GetAnother, self._get_another) + self.reset_service = rospy.Service( + '~reset', Trigger, self._reset) + self.dyn_srv = Server( + DualarmGraspSegmentationConfig, self._dyn_callback) + + def subscribe(self): + # larger buff_size is necessary for taking time callback + # http://stackoverflow.com/questions/26415699/ros-subscriber-not-up-to-date/29160379#29160379 # NOQA + queue_size = rospy.get_param('~queue_size', 10) + use_mask = rospy.get_param('~use_mask', True) + if use_mask: + sub = message_filters.Subscriber( + '~input', Image, queue_size=1, buff_size=2**24) + sub_mask = message_filters.Subscriber( + '~input/mask', Image, queue_size=1, buff_size=2**24) + self.subs = [sub, sub_mask] + if rospy.get_param('~approximate_sync', False): + slop = rospy.get_param('~slop', 0.1) + sync = message_filters.ApproximateTimeSynchronizer( + self.subs, queue_size=queue_size, slop=slop) + else: + sync = message_filters.TimeSynchronizer( + self.subs, queue_size=queue_size) + sync.registerCallback(self._recognize) + else: + sub = rospy.Subscriber( + '~input', Image, callback=self._recognize, + queue_size=queue_size, buff_size=2**24) + + def unsubscribe(self): + for sub in self.subs: + sub.unregister() + + def _recognize(self, imgmsg, mask_msg=None): + bridge = cv_bridge.CvBridge() + bgr = bridge.imgmsg_to_cv2(imgmsg, desired_encoding='bgr8') + bgr = cv2.resize(bgr, (640, 480)) + if mask_msg is not None: + mask = bridge.imgmsg_to_cv2(mask_msg) + mask = cv2.resize(mask, (640, 480)) + if mask.shape != bgr.shape[:2]: + logerr_throttle(10, + 'Size of input image and mask is different') + return + elif mask.size == 0: + logerr_throttle(10, 'Size of input mask is 0') + return + bgr[mask < 128] = self.mean_bgr + + blob = (bgr - self.mean_bgr).transpose((2, 0, 1)) + x_data = np.array([blob], dtype=np.float32) + if self.gpu != -1: + x_data = chainer.cuda.to_gpu(x_data, device=self.gpu) + x = Variable(x_data) + self.model(x) + + score = self.model.score.array[0] + sg_score = self.model.single_grasp_score.array[0] + dg_score = self.model.dual_grasp_score.array[0] + + # segmentation + seg_prob = chainer.functions.softmax(score, axis=0) + seg_prob = seg_prob.array + n_labels = len(self.label_names) + for lbl in range(n_labels): + if lbl not in self.candidates: + seg_prob[lbl, :, :] = 0.0 + seg_prob = seg_prob / cupy.atleast_3d(seg_prob.sum(axis=0)[None, :, :]) + seg_prob[seg_prob < self.score_thresh] = 0.0 + label_pred = seg_prob.argmax(axis=0) + + # singlearm grasp + sg_probs = chainer.functions.softmax(sg_score, axis=0) + sg_probs = sg_probs.array + sg_grasp = sg_probs[1, :, :] + sg_grasp = seg_prob * sg_grasp[None] + sg_label_prob = sg_grasp.max(axis=(1, 2)) + sg_labels = sg_label_prob.argsort()[::-1] + sg_label = None + for lbl in sg_labels: + if lbl not in self.giveup_labels['single']: + sg_label = lbl + break + if sg_label == 0 or sg_label is None: + sg_mask = cupy.zeros(sg_grasp.shape[1:], dtype=np.int32) + else: + sg_mask = sg_probs[1, :, :] + sg_mask[seg_prob[sg_label] < self.score_thresh] = 0.0 + # if self.grasp_thresh > sg_mask.max(): + sg_thresh = sg_mask.max() - 0.05 + # else: + # sg_thresh = self.grasp_thresh + sg_mask = (sg_mask > sg_thresh).astype(np.int32) + sg_mask = sg_mask * 255 + + # dualarm grasp + dg_probs = chainer.functions.softmax(dg_score, axis=0) + dg_probs = dg_probs.array + dg_grasp = dg_probs[1, :, :] + dg_grasp = seg_prob * dg_grasp[None] + dg_label_prob = dg_grasp.max(axis=(1, 2)) + dg_labels = dg_label_prob.argsort()[::-1] + dg_label = None + for lbl in dg_labels: + if lbl not in self.giveup_labels['dual']: + dg_label = lbl + break + if dg_label == 0 or dg_label is None: + dg_mask = cupy.zeros(dg_grasp.shape[1:], dtype=np.int32) + else: + dg_mask = dg_probs[1, :, :] + dg_mask[seg_prob[dg_label] < self.score_thresh] = 0.0 + # if self.grasp_thresh > dg_mask.max(): + dg_thresh = dg_mask.max() - 0.05 + # else: + # dg_thresh = self.grasp_thresh + dg_mask = (dg_mask > dg_thresh).astype(np.int32) + dg_mask = dg_mask * 255 + + # GPU -> CPU + seg_prob = chainer.cuda.to_cpu(seg_prob) + label_pred = chainer.cuda.to_cpu(label_pred) + sg_probs = chainer.cuda.to_cpu(sg_probs) + sg_grasp = chainer.cuda.to_cpu(sg_grasp) + sg_mask = chainer.cuda.to_cpu(sg_mask) + sg_label = np.asscalar(chainer.cuda.to_cpu(sg_label)) + sg_label_prob = chainer.cuda.to_cpu(sg_label_prob) + dg_probs = chainer.cuda.to_cpu(dg_probs) + dg_grasp = chainer.cuda.to_cpu(dg_grasp) + dg_mask = chainer.cuda.to_cpu(dg_mask) + dg_label = np.asscalar(chainer.cuda.to_cpu(dg_label)) + dg_label_prob = chainer.cuda.to_cpu(dg_label_prob) + + # msg + input_msg = bridge.cv2_to_imgmsg( + bgr.astype(np.uint8), encoding='bgr8') + input_msg.header = imgmsg.header + seg_msg = bridge.cv2_to_imgmsg(seg_prob.astype(np.float32)) + seg_msg.header = imgmsg.header + label_msg = bridge.cv2_to_imgmsg( + label_pred.astype(np.int32), '32SC1') + label_msg.header = imgmsg.header + + sg_grasp_msg = bridge.cv2_to_imgmsg(sg_grasp.astype(np.float32)) + sg_grasp_msg.header = imgmsg.header + sg_mask_msg = bridge.cv2_to_imgmsg( + sg_mask.astype(np.uint8), 'mono8') + sg_mask_msg.header = imgmsg.header + sg_label_msg = ClassificationResult( + header=imgmsg.header, + labels=[sg_label], + label_names=[self.label_names[sg_label]], + label_proba=[sg_label_prob[sg_label]], + probabilities=sg_label_prob, + classifier='DualarmGraspFCN32s', + target_names=self.label_names) + + dg_grasp_msg = bridge.cv2_to_imgmsg(dg_grasp.astype(np.float32)) + dg_grasp_msg.header = imgmsg.header + dg_mask_msg = bridge.cv2_to_imgmsg( + dg_mask.astype(np.uint8), 'mono8') + dg_mask_msg.header = imgmsg.header + dg_label_msg = ClassificationResult( + header=imgmsg.header, + labels=[dg_label], + label_names=[self.label_names[dg_label]], + label_proba=[dg_label_prob[dg_label]], + probabilities=dg_label_prob, + classifier='DualarmGraspFCN32s', + target_names=self.label_names) + + if self.sampling: + if len(self.candidates) != 2: + rospy.logerr('Invalid tote contents num: {}'.format( + self.candidates)) + style = self.grasping_way + label_mask = label_pred == self.candidates[1] + if style == 'single': + class_msg = sg_label_msg + sample_mask = self._random_sample_single( + sg_probs[1, :, :], label_mask) + elif style == 'dual': + class_msg = dg_label_msg + sample_mask = self._random_sample_dual( + dg_probs[1, :, :], label_mask) + else: + rospy.logerr('Invalid sampling style: {}'.format(style)) + mask_msg = bridge.cv2_to_imgmsg( + sample_mask.astype(np.uint8), 'mono8') + mask_msg.header = imgmsg.header + else: + # select grasp style + if sg_label_prob[sg_label] > dg_label_prob[dg_label]: + style = 'single' + class_msg = sg_label_msg + mask_msg = sg_mask_msg + else: + style = 'dual' + class_msg = dg_label_msg + mask_msg = dg_mask_msg + grasp_class = GraspClassificationResult() + grasp_class.header = imgmsg.header + grasp_class.style = style + grasp_class.is_target = True + grasp_class.classification = class_msg + + # publish + self.pub_input.publish(input_msg) + self.pub_seg.publish(seg_msg) + self.pub_label.publish(label_msg) + + self.pub_sg_grasp.publish(sg_grasp_msg) + self.pub_sg_mask.publish(sg_mask_msg) + self.pub_sg_label.publish(sg_label_msg) + + self.pub_dg_grasp.publish(dg_grasp_msg) + self.pub_dg_mask.publish(dg_mask_msg) + self.pub_dg_label.publish(dg_label_msg) + + self.pub_mask.publish(mask_msg) + self.pub_grasp.publish(grasp_class) + time.sleep(1.0) + + def _get_another(self, req): + grasp_style = req.style + label = req.label + self.giveup_labels[grasp_style].append(label) + response = GetAnotherResponse() + response.success = True + return response + + def _reset(self, req): + self.giveup_labels = { + 'single': [self.bg_index], + 'dual': [self.bg_index], + } + response = TriggerResponse() + response.success = True + return response + + def _dyn_callback(self, config, level): + self.score_thresh = config.score_thresh + self.grasp_thresh = config.grasp_thresh + self.sampling_thresh = config.sampling_thresh + self.grasping_way = config.grasping_way + return config + + def _random_sample_single(self, sg_prob, label_mask): + weight = sg_prob + weight[~label_mask] = 0.0 + if self.sampling_weighted and np.sum(weight) > 0: + weight = weight.ravel() / np.sum(weight) + elif np.sum(label_mask) > 0: + label_mask = label_mask.astype(np.float32) + weight = label_mask.ravel() / np.sum(label_mask) + else: + weight = None + sample_grasp = np.zeros(sg_prob.shape) + sampled_i = np.random.choice(sg_prob.size, p=weight) + sampled_index = ( + sampled_i // sg_prob.shape[1], + sampled_i % sg_prob.shape[1], + ) + sample_grasp[sampled_index] = 255 + sample_grasp = scipy.ndimage.filters.gaussian_filter( + sample_grasp, sigma=20) + sample_grasp = sample_grasp / sample_grasp.max() + sample_mask = sample_grasp > self.sampling_thresh + sample_mask = sample_mask.astype(np.uint8) * 255 + return sample_mask + + def _random_sample_dual(self, dg_prob, label_mask): + indices = np.column_stack(np.where(label_mask)) + + c_masks = [] + if indices.size > 1: + kmeans = KMeans(n_clusters=2) + try: + kmeans.fit(indices) + centers = kmeans.cluster_centers_ + labels = kmeans.labels_ + for label, center in enumerate(centers): + center = np.round(center).astype(np.int32) + c_mask = np.zeros(label_mask.shape).astype(bool) + masked_indices = indices[labels == label] + masked_indices = ( + masked_indices[:, 0], masked_indices[:, 1]) + c_mask[masked_indices] = True + weight = dg_prob.copy() + weight[~c_mask] = 0.0 + if self.sampling_weighted and np.sum(weight) > 0: + weight = weight.ravel() / np.sum(weight) + elif np.sum(label_mask) > 0: + label_mask = label_mask.astype(np.float32) + weight = label_mask.ravel() / np.sum(label_mask) + else: + weight = None + dual_giveup = False + trial_num = 10 + for i in range(0, trial_num): + sampled_i = np.random.choice(dg_prob.size, p=weight) + sampled_index = ( + sampled_i // dg_prob.shape[1], + sampled_i % dg_prob.shape[1], + ) + c_grasp = np.zeros(dg_prob.shape) + c_grasp[sampled_index] = 255 + c_grasp = scipy.ndimage.filters.gaussian_filter( + c_grasp, sigma=20) + c_grasp = c_grasp / c_grasp.max() + c_mask = c_grasp > self.sampling_thresh + if len(c_masks) > 0: + if not np.any(np.logical_and(c_mask, c_masks[0])): + c_masks.append(c_mask) + break + else: + c_masks.append(c_mask) + break + if i == trial_num - 1: + dual_giveup = True + except Exception: + dual_giveup = True + if len(c_masks) == 2 and dual_giveup is False: + sample_mask = np.logical_or(c_masks[0], c_masks[1]) + sample_mask = sample_mask.astype(np.uint8) * 255 + else: + sample_mask = np.zeros(dg_prob.shape, dtype=np.uint8) + + return sample_mask + + +if __name__ == '__main__': + rospy.init_node('dualarm_grasp_segmentation') + node = DualarmGraspSegmentation() + rospy.spin() diff --git a/demos/selective_dualarm_grasping/node_scripts/dualarm_occluded_grasp_instance_segmentation.py b/demos/selective_dualarm_grasping/node_scripts/dualarm_occluded_grasp_instance_segmentation.py new file mode 100755 index 000000000..38be5f363 --- /dev/null +++ b/demos/selective_dualarm_grasping/node_scripts/dualarm_occluded_grasp_instance_segmentation.py @@ -0,0 +1,785 @@ +#!/usr/bin/env python + +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + +import easydict +import matplotlib +import numpy as np +import os.path as osp +# import time +import yaml + +matplotlib.use('Agg') # NOQA + +import chainer +import cv_bridge +from dynamic_reconfigure.server import Server +from jsk_recognition_msgs.msg import ClassificationResult +from jsk_recognition_msgs.msg import ClusterPointIndices +from jsk_recognition_msgs.msg import Label +from jsk_recognition_msgs.msg import LabelArray +from jsk_recognition_msgs.msg import Rect +from jsk_recognition_msgs.msg import RectArray +from jsk_topic_tools import ConnectionBasedTransport +import matplotlib.pyplot as plt +import message_filters +from pcl_msgs.msg import PointIndices +import rospy +import scipy +from sensor_msgs.msg import Image +from sklearn.cluster import KMeans +from std_srvs.srv import Trigger +from std_srvs.srv import TriggerResponse + +from grasp_data_generator.models import OccludedGraspMaskRCNNResNet101 +from grasp_data_generator.visualizations \ + import vis_occluded_grasp_instance_segmentation + +from dualarm_grasping.cfg \ + import DualarmOccludedGraspInstanceSegmentationConfig +from dualarm_grasping.msg import GraspClassificationResult +from dualarm_grasping.srv import GetAnother +from dualarm_grasping.srv import GetAnotherResponse + + +filepath = osp.dirname(osp.realpath(__file__)) + + +class DualarmOccludedGraspInstanceSegmentation(ConnectionBasedTransport): + + def __init__(self): + super(self.__class__, self).__init__() + self.gpu = rospy.get_param('~gpu', -1) + model_file = rospy.get_param('~model_file') + + self.label_names = rospy.get_param('~label_names') + self.bg_index = rospy.get_param('~bg_index', -1) + if self.bg_index >= 0: + bg_label_name = self.label_names[self.bg_index] + if bg_label_name != '__background__': + rospy.logerr('bg_label_name is not __background__: {}' + .format(bg_label_name)) + + self.sampling = rospy.get_param('~sampling', False) + self.sampling_weighted = rospy.get_param('~sampling_weighted', False) + cfgpath = rospy.get_param( + '~config_yaml', osp.join(filepath, '../yaml/config.yaml')) + with open(cfgpath, 'r') as f: + config = easydict.EasyDict(yaml.load(f)) + + tote_contents = rospy.get_param('~tote_contents', None) + self.candidates = [self.bg_index] + if tote_contents is None: + self.candidates += range(len(self.label_names)) + else: + self.candidates += [ + self.label_names.index(x) for x in tote_contents] + self.candidates = sorted(list(set(self.candidates))) + + self.giveup_ins_ids = { + 'single': [], + 'dual': [], + } + + self.target_grasp = rospy.get_param('~target_grasp', False) + target_names = rospy.get_param('~target_names', None) + if self.target_grasp and target_names is None: + target_names = self.label_names + self.target_ids = [ + self.label_names.index(x) for x in target_names] + + # chainer global config + chainer.global_config.train = False + chainer.global_config.enable_backprop = False + + # mask rcnn + if 'rotate_angle' not in config: + self.rotate_angle = None + else: + self.rotate_angle = config.rotate_angle + self.model = OccludedGraspMaskRCNNResNet101( + n_fg_class=len(self.label_names), + anchor_scales=config.anchor_scales, + min_size=config.min_size, + max_size=config.max_size, + rpn_dim=config.rpn_dim, + rotate_angle=self.rotate_angle) + chainer.serializers.load_npz(model_file, self.model) + + if self.gpu != -1: + chainer.cuda.get_device_from_id(self.gpu).use() + self.model.to_gpu() + + # input + self.pub_net_input = self.advertise( + '~debug/net_input', Image, queue_size=1) + self.pub_vis_output = self.advertise( + '~debug/vis_output', Image, queue_size=1) + # vis + self.pub_vis_cpi = self.advertise( + '~output/vis/cluster_indices', ClusterPointIndices, queue_size=1) + self.pub_vis_labels = self.advertise( + '~output/vis/labels', LabelArray, queue_size=1) + self.pub_vis_cls_lbl = self.advertise( + '~output/vis/cls_label', Image, queue_size=1) + self.pub_vis_ins_lbl = self.advertise( + '~output/vis/ins_label', Image, queue_size=1) + + # occ + self.pub_occ_cpi = self.advertise( + '~output/occ/cluster_indices', ClusterPointIndices, queue_size=1) + self.pub_occ_labels = self.advertise( + '~output/occ/labels', LabelArray, queue_size=1) + self.pub_occ_cls_lbl = self.advertise( + '~output/occ/cls_label', Image, queue_size=1) + self.pub_occ_ins_lbl = self.advertise( + '~output/occ/ins_label', Image, queue_size=1) + + # bbox + self.pub_rects = self.advertise( + "~output/rects", RectArray, queue_size=1) + + # class + self.pub_class = self.advertise( + "~output/class", ClassificationResult, + queue_size=1) + + # single + self.pub_sg_cpi = self.advertise( + '~output/single/cluster_indices', + ClusterPointIndices, queue_size=1) + self.pub_sg_labels = self.advertise( + '~output/single/labels', LabelArray, queue_size=1) + self.pub_sg_cls_lbl = self.advertise( + '~output/single/cls_label', Image, queue_size=1) + self.pub_sg_ins_lbl = self.advertise( + '~output/single/ins_label', Image, queue_size=1) + + # dual + self.pub_dg_cpi = self.advertise( + '~output/dual/cluster_indices', + ClusterPointIndices, queue_size=1) + self.pub_dg_labels = self.advertise( + '~output/dual/labels', LabelArray, queue_size=1) + self.pub_dg_cls_lbl = self.advertise( + '~output/dual/cls_label', Image, queue_size=1) + self.pub_dg_ins_lbl = self.advertise( + '~output/dual/ins_label', Image, queue_size=1) + + # output + self.pub_grasp_mask = self.advertise( + '~output/grasp_mask', Image, queue_size=1) + self.pub_grasp_class = self.advertise( + '~output/grasp_class', GraspClassificationResult, queue_size=1) + + self.get_another_service = rospy.Service( + '~get_another', GetAnother, self._get_another) + self.reset_service = rospy.Service( + '~reset', Trigger, self._reset) + self.dyn_srv = Server( + DualarmOccludedGraspInstanceSegmentationConfig, + self._dyn_callback) + + def subscribe(self): + self.model.score_thresh = self.score_thresh + self.model.nms_thresh = self.nms_thresh + + # larger buff_size is necessary for taking time callback + # http://stackoverflow.com/questions/26415699/ros-subscriber-not-up-to-date/29160379#29160379 # NOQA + queue_size = rospy.get_param('~queue_size', 10) + self.use_mask = rospy.get_param('~use_mask', True) + if self.use_mask: + sub = message_filters.Subscriber( + '~input', Image, queue_size=1, buff_size=2**24) + sub_mask = message_filters.Subscriber( + '~input/mask', Image, queue_size=1, buff_size=2**24) + self.subs = [sub, sub_mask] + if rospy.get_param('~approximate_sync', False): + slop = rospy.get_param('~slop', 0.1) + sync = message_filters.ApproximateTimeSynchronizer( + self.subs, queue_size=queue_size, slop=slop) + else: + sync = message_filters.TimeSynchronizer( + self.subs, queue_size=queue_size) + sync.registerCallback(self._recognize) + else: + sub = rospy.Subscriber( + '~input', Image, callback=self._recognize, + queue_size=queue_size, buff_size=2**24) + + def unsubscribe(self): + for sub in self.subs: + sub.unregister() + + def _recognize(self, img_msg, mask_msg=None): + bridge = cv_bridge.CvBridge() + rgb = bridge.imgmsg_to_cv2(img_msg, desired_encoding='rgb8') + if self.use_mask: + if mask_msg is not None: + mask = bridge.imgmsg_to_cv2(mask_msg) + # rgb[mask < 128] = self.model.mean.flatten() + # H, W, C -> C, H, W + img = rgb.transpose((2, 0, 1)) + results = self.model.predict([img], return_probs=True) + ins_labels, ins_probs, labels, bboxes, scores = results[:5] + sg_labels, sg_probs, dg_labels, dg_probs = results[5:] + try: + ins_label, ins_prob, label, bbox, score = \ + ins_labels[0], ins_probs[0], labels[0], bboxes[0], scores[0] + sg_label, sg_prob, dg_label, dg_prob = \ + sg_labels[0], sg_probs[0], dg_labels[0], dg_probs[0] + except IndexError: + rospy.logerr('no predicts returned') + return + + # matplot + fig, axes = plt.subplots( + 1, 5, sharey=True, figsize=(100, 20), dpi=120) + vis_occluded_grasp_instance_segmentation( + img, ins_label, label, bbox, score, + sg_label, dg_label, self.label_names, + rotate_angle=self.rotate_angle, axes=axes, + linewidth=5.0, fontsize=30) + fig.canvas.draw() + w, h = fig.canvas.get_width_height() + vis_output_img = np.fromstring( + fig.canvas.tostring_rgb(), dtype=np.uint8) + fig.clf() + vis_output_img.shape = (h, w, 3) + plt.close() + vis_output_msg = bridge.cv2_to_imgmsg( + vis_output_img, encoding='rgb8') + vis_output_msg.header = img_msg.header + + # msg + # input + net_input_msg = bridge.cv2_to_imgmsg( + rgb.astype(np.uint8), encoding='rgb8') + net_input_msg.header = img_msg.header + + # vis lbls + vis_cpi_msg = ClusterPointIndices(header=img_msg.header) + vis_labels_msg = LabelArray(header=img_msg.header) + vis_cls_lbl = - np.ones(img.shape[1:], dtype=np.int32) + vis_ins_lbl = - np.ones(img.shape[1:], dtype=np.int32) + vis_ins_n_pixel = [] + + # occ lbls + occ_cpi_msg = ClusterPointIndices(header=img_msg.header) + occ_labels_msg = LabelArray(header=img_msg.header) + occ_cls_lbl = - np.ones(img.shape[1:], dtype=np.int32) + occ_ins_lbl = - np.ones(img.shape[1:], dtype=np.int32) + occ_ins_n_pixel = [] + + for ins_id, (cls_id, ins_lbl) in enumerate(zip(label, ins_label)): + # vis region + if self.use_mask: + vis_msk = np.logical_and(ins_lbl == 1, mask > 128) + else: + vis_msk = ins_lbl == 1 + vis_ins_n_pixel.append(vis_msk.sum()) + + # occ region + if self.use_mask: + occ_msk = np.logical_and(ins_lbl == 2, mask > 128) + else: + occ_msk = ins_lbl == 2 + occ_ins_n_pixel.append(occ_msk.sum()) + + if cls_id not in self.candidates: + continue + + # vis region + class_name = self.label_names[cls_id] + vis_indices = np.where(vis_msk.flatten())[0] + vis_indices_msg = PointIndices( + header=img_msg.header, indices=vis_indices) + vis_cpi_msg.cluster_indices.append(vis_indices_msg) + vis_labels_msg.labels.append( + Label(id=cls_id, name=class_name)) + vis_cls_lbl[vis_msk] = cls_id + vis_ins_lbl[vis_msk] = ins_id + + # occ region + occ_indices = np.where(occ_msk.flatten())[0] + occ_indices_msg = PointIndices( + header=img_msg.header, indices=occ_indices) + occ_cpi_msg.cluster_indices.append(occ_indices_msg) + occ_labels_msg.labels.append( + Label(id=cls_id, name=class_name)) + occ_cls_lbl[occ_msk] = cls_id + occ_ins_lbl[occ_msk] = ins_id + + vis_cls_lbl_msg = bridge.cv2_to_imgmsg(vis_cls_lbl) + vis_cls_lbl_msg.header = img_msg.header + vis_ins_lbl_msg = bridge.cv2_to_imgmsg(vis_ins_lbl) + vis_ins_lbl_msg.header = img_msg.header + vis_ins_n_pixel = np.array(vis_ins_n_pixel, dtype=np.int32) + + occ_cls_lbl_msg = bridge.cv2_to_imgmsg(occ_cls_lbl) + occ_cls_lbl_msg.header = img_msg.header + occ_ins_lbl_msg = bridge.cv2_to_imgmsg(occ_ins_lbl) + occ_ins_lbl_msg.header = img_msg.header + occ_ins_n_pixel = np.array(occ_ins_n_pixel, dtype=np.int32) + + # bbox + rects_msg = RectArray(header=img_msg.header) + for bb in bbox: + rect = Rect(x=bb[1], y=bb[0], + width=bb[3] - bb[1], + height=bb[2] - bb[0]) + rects_msg.rects.append(rect) + + # classification + cls_msg = ClassificationResult( + header=img_msg.header, + classifier='OccludedGraspMaskRCNNResNet101', + target_names=self.label_names, + labels=label, + label_names=[self.label_names[lbl] for lbl in label], + label_proba=score, + ) + + # sg, dg + sg_cpi_msg = ClusterPointIndices(header=img_msg.header) + sg_labels_msg = LabelArray(header=img_msg.header) + sg_cls_lbl = - np.ones(img.shape[1:], dtype=np.int32) + sg_ins_lbl = - np.ones(img.shape[1:], dtype=np.int32) + dg_cpi_msg = ClusterPointIndices(header=img_msg.header) + dg_labels_msg = LabelArray(header=img_msg.header) + dg_cls_lbl = - np.ones(img.shape[1:], dtype=np.int32) + dg_ins_lbl = - np.ones(img.shape[1:], dtype=np.int32) + + for ins_id, (cls_id, sg_lbl, dg_lbl) in enumerate( + zip(label, sg_label, dg_label)): + if cls_id not in self.candidates: + continue + class_name = self.label_names[cls_id] + + # sg + if self.use_mask: + sg_msk = np.logical_and(sg_lbl > 0, mask > 128) + else: + sg_msk = sg_lbl > 0 + sg_indices = np.where(sg_msk.flatten())[0] + sg_indices_msg = PointIndices( + header=img_msg.header, indices=sg_indices) + sg_cpi_msg.cluster_indices.append(sg_indices_msg) + sg_labels_msg.labels.append( + Label(id=cls_id, name=class_name)) + sg_cls_lbl[sg_msk] = cls_id + sg_ins_lbl[sg_msk] = ins_id + + # dg + if self.use_mask: + dg_msk = np.logical_and(dg_lbl > 0, mask > 128) + else: + dg_msk = dg_lbl > 0 + dg_indices = np.where(dg_msk.flatten())[0] + dg_indices_msg = PointIndices( + header=img_msg.header, indices=dg_indices) + dg_cpi_msg.cluster_indices.append(dg_indices_msg) + dg_labels_msg.labels.append( + Label(id=cls_id, name=class_name)) + dg_cls_lbl[dg_msk] = cls_id + dg_ins_lbl[dg_msk] = ins_id + + sg_cls_lbl_msg = bridge.cv2_to_imgmsg(sg_cls_lbl) + sg_cls_lbl_msg.header = img_msg.header + sg_ins_lbl_msg = bridge.cv2_to_imgmsg(sg_ins_lbl) + sg_ins_lbl_msg.header = img_msg.header + dg_cls_lbl_msg = bridge.cv2_to_imgmsg(dg_cls_lbl) + dg_cls_lbl_msg.header = img_msg.header + dg_ins_lbl_msg = bridge.cv2_to_imgmsg(dg_ins_lbl) + dg_ins_lbl_msg.header = img_msg.header + + sg_ins_prob_img = ins_prob[:, 1, :, :] * \ + np.sum(sg_prob[:, 1:, :, :], axis=1) + if self.use_mask: + sg_ins_prob_mask = np.repeat( + (mask <= 128)[None], len(sg_ins_prob_img), axis=0) + sg_ins_prob_img[sg_ins_prob_mask] = 0 + sg_ins_prob = np.max(sg_ins_prob_img, axis=(1, 2)) + assert len(sg_ins_prob) == len(sg_prob) + assert sg_ins_prob.ndim == 1 + + dg_ins_prob_img = ins_prob[:, 1, :, :] * \ + np.sum(dg_prob[:, 1:, :, :], axis=1) + if self.use_mask: + dg_ins_prob_mask = np.repeat( + (mask <= 128)[None], len(dg_ins_prob_img), axis=0) + dg_ins_prob_img[dg_ins_prob_mask] = 0 + dg_ins_prob = np.max(dg_ins_prob_img, axis=(1, 2)) + assert len(dg_ins_prob) == len(dg_prob) + assert dg_ins_prob.ndim == 1 + + # grasp mask and style + if self.sampling: + if len(self.candidates) != 2: + rospy.logerr('Invalid tote contents num: {}'.format( + self.candidates)) + grasp_style = self.grasping_way + if self.candidates[1] in label: + grasp_cls_ids = [self.candidates[1]] + if grasp_style == 'single': + grasp_probs = sg_ins_prob[label == grasp_cls_ids[0]][0:1] + grasp_mask = self._random_sample_sg_mask( + sg_cls_lbl == grasp_cls_ids[0], + vis_cls_lbl == grasp_cls_ids[0]) + else: + grasp_probs = dg_ins_prob[label == grasp_cls_ids[0]][0:1] + grasp_mask = self._random_sample_dg_mask( + dg_cls_lbl == grasp_cls_ids[0], + vis_cls_lbl == grasp_cls_ids[0]) + else: + grasp_cls_ids = [] + grasp_probs = [] + grasp_mask = np.zeros(img.shape[1:], dtype=np.uint8) + is_target = True + else: + sg_ins_prob[sg_ins_prob <= self.grasp_thresh] = 0 + dg_ins_prob[dg_ins_prob <= self.grasp_thresh] = 0 + vis_ratio = vis_ins_n_pixel / (vis_ins_n_pixel + occ_ins_n_pixel) + vis_ratio[np.isnan(vis_ratio)] = 0 + target_ins_ids = [] + if self.target_grasp: + for ins_id in range(len(vis_ratio)): + if (label[ins_id] in self.target_ids and + (sg_ins_prob[ins_id] > 0 or + dg_ins_prob[ins_id] > 0)): + target_ins_ids.append(ins_id) + target_ins_ids = np.array(target_ins_ids, dtype=np.int32) + + if self.target_grasp and len(target_ins_ids) > 0: + target_sg_ins_prob = sg_ins_prob[target_ins_ids] + target_dg_ins_prob = dg_ins_prob[target_ins_ids] + + sg_ins_prob_max = target_sg_ins_prob.max() + dg_ins_prob_max = target_dg_ins_prob.max() + sg_ins_id = target_ins_ids[target_sg_ins_prob.argmax()] + dg_ins_id = target_ins_ids[target_dg_ins_prob.argmax()] + + # either of sg, dg are graspable + if vis_ratio[sg_ins_id] > self.vis_thresh \ + or vis_ratio[dg_ins_id] > self.vis_thresh: + # both of sg, dg are graspable + if vis_ratio[sg_ins_id] > self.vis_thresh \ + and vis_ratio[dg_ins_id] > self.vis_thresh: + if sg_ins_prob_max > dg_ins_prob_max: + grasp_style = 'single' + else: + grasp_style = 'dual' + # sg is grapable, but dg is not + elif vis_ratio[sg_ins_id] > self.vis_thresh: + grasp_style = 'single' + # dg is grapable, but sg is not + else: + grasp_style = 'dual' + + if grasp_style == 'single': + grasp_ins_ids = [sg_ins_id] + grasp_probs = [sg_ins_prob_max] + grasp_mask = \ + sg_ins_prob_img[sg_ins_id] > self.grasp_thresh + else: + grasp_ins_ids = [dg_ins_id] + grasp_probs = [dg_ins_prob_max] + grasp_mask = \ + dg_ins_prob_img[dg_ins_id] > self.grasp_thresh + # none of sg, dg are graspable + else: + if sg_ins_prob_max > dg_ins_prob_max: + occ_ins_id = self._find_occ_top( + sg_ins_id, ins_label, label) + else: + occ_ins_id = self._find_occ_top( + dg_ins_id, ins_label, label) + + if sg_ins_prob[occ_ins_id] > dg_ins_prob[occ_ins_id]: + grasp_style = 'single' + grasp_ins_ids = [occ_ins_id] + grasp_probs = [sg_ins_prob[occ_ins_id]] + grasp_mask = \ + sg_ins_prob_img[occ_ins_id] > self.grasp_thresh + else: + grasp_style = 'dual' + grasp_ins_ids = [occ_ins_id] + grasp_probs = [dg_ins_prob[occ_ins_id]] + grasp_mask = \ + dg_ins_prob_img[occ_ins_id] > self.grasp_thresh + else: + sg_ins_prob[vis_ratio <= self.vis_thresh] = 0 + dg_ins_prob[vis_ratio <= self.vis_thresh] = 0 + is_candidates = np.array( + [lbl in self.candidates for lbl in label], dtype=np.bool) + sg_ins_prob[~is_candidates] = 0 + dg_ins_prob[~is_candidates] = 0 + sg_ins_prob_max = sg_ins_prob.max() + dg_ins_prob_max = dg_ins_prob.max() + + if sg_ins_prob_max > dg_ins_prob_max: + grasp_style = 'single' + grasp_ins_ids = [np.argmax(sg_ins_prob)] + grasp_probs = [sg_ins_prob_max] + if label[grasp_ins_ids[0]] in self.candidates: + grasp_mask = sg_ins_prob_img[ + grasp_ins_ids[0]] > self.grasp_thresh + else: + grasp_mask = np.zeros(img.shape[1:], dtype=np.uint8) + else: + grasp_style = 'dual' + grasp_ins_ids = [np.argmax(dg_ins_prob)] + grasp_probs = [dg_ins_prob_max] + if label[grasp_ins_ids[0]] in self.candidates: + grasp_mask = dg_ins_prob_img[ + grasp_ins_ids[0]] > self.grasp_thresh + else: + grasp_mask = np.zeros(img.shape[1:], dtype=np.uint8) + + grasp_mask = grasp_mask.astype(np.uint8) * 255 + grasp_cls_ids = [ + label[grasp_ins_id] for grasp_ins_id in grasp_ins_ids] + if self.target_grasp: + is_target = grasp_cls_ids[0] in self.target_ids + else: + is_target = True + + grasp_mask_msg = bridge.cv2_to_imgmsg(grasp_mask, encoding='mono8') + grasp_mask_msg.header = img_msg.header + + grasp_label_names = [] + for grasp_cls_id in grasp_cls_ids: + if grasp_cls_id >= 0: + grasp_label_names.append(self.label_names[grasp_cls_id]) + else: + grasp_label_names.append('__background__') + + grasp_cls_msg = GraspClassificationResult( + header=img_msg.header, + style=grasp_style, + is_target=is_target, + classification=ClassificationResult( + header=img_msg.header, + labels=grasp_cls_ids, + label_names=grasp_label_names, + label_proba=grasp_probs, + classifier='OccludedGraspMaskRCNNResNet101', + target_names=self.label_names)) + + # publish + # input + self.pub_net_input.publish(net_input_msg) + self.pub_vis_output.publish(vis_output_msg) + + # vis + self.pub_vis_cpi.publish(vis_cpi_msg) + self.pub_vis_labels.publish(vis_labels_msg) + self.pub_vis_cls_lbl.publish(vis_cls_lbl_msg) + self.pub_vis_ins_lbl.publish(vis_ins_lbl_msg) + + # occ + self.pub_occ_cpi.publish(occ_cpi_msg) + self.pub_occ_labels.publish(occ_labels_msg) + self.pub_occ_cls_lbl.publish(occ_cls_lbl_msg) + self.pub_occ_ins_lbl.publish(occ_ins_lbl_msg) + + # bbox + self.pub_rects.publish(rects_msg) + + # class + self.pub_class.publish(cls_msg) + + # sg + self.pub_sg_cpi.publish(sg_cpi_msg) + self.pub_sg_labels.publish(sg_labels_msg) + self.pub_sg_cls_lbl.publish(sg_cls_lbl_msg) + self.pub_sg_ins_lbl.publish(sg_ins_lbl_msg) + + # dg + self.pub_dg_cpi.publish(dg_cpi_msg) + self.pub_dg_labels.publish(dg_labels_msg) + self.pub_dg_cls_lbl.publish(dg_cls_lbl_msg) + self.pub_dg_ins_lbl.publish(dg_ins_lbl_msg) + + self.pub_grasp_mask.publish(grasp_mask_msg) + self.pub_grasp_class.publish(grasp_cls_msg) + # time.sleep(1.0) + + def _random_sample_sg_mask(self, sg_prb, label_msk): + weight = sg_prb + weight[~label_msk] = 0.0 + if self.sampling_weighted and np.sum(weight) > 0: + weight = weight.ravel() / np.sum(weight) + elif np.sum(label_msk) > 0: + label_msk = label_msk.astype(np.float32) + weight = label_msk.ravel() / np.sum(label_msk) + else: + weight = None + sample_grasp = np.zeros(sg_prb.shape) + sampled_i = np.random.choice(sg_prb.size, p=weight) + sampled_index = ( + sampled_i // sg_prb.shape[1], + sampled_i % sg_prb.shape[1], + ) + sample_grasp[sampled_index] = 255 + sample_grasp = scipy.ndimage.filters.gaussian_filter( + sample_grasp, sigma=20) + sample_grasp = sample_grasp / sample_grasp.max() + sample_mask = sample_grasp > self.sampling_thresh + sample_mask = sample_mask.astype(np.uint8) * 255 + return sample_mask + + def _random_sample_dg_mask(self, dg_prb, label_msk): + indices = np.column_stack(np.where(label_msk)) + + c_masks = [] + if indices.size > 1: + kmeans = KMeans(n_clusters=2) + try: + kmeans.fit(indices) + centers = kmeans.cluster_centers_ + labels = kmeans.labels_ + for label, center in enumerate(centers): + center = np.round(center).astype(np.int32) + c_mask = np.zeros(label_msk.shape).astype(bool) + masked_indices = indices[labels == label] + masked_indices = ( + masked_indices[:, 0], masked_indices[:, 1]) + c_mask[masked_indices] = True + weight = dg_prb.copy() + weight[~c_mask] = 0.0 + if self.sampling_weighted and np.sum(weight) > 0: + weight = weight.ravel() / np.sum(weight) + elif np.sum(label_msk) > 0: + label_msk = label_msk.astype(np.float32) + weight = label_msk.ravel() / np.sum(label_msk) + else: + weight = None + dual_giveup = False + trial_num = 10 + for i in range(0, trial_num): + sampled_i = np.random.choice(dg_prb.size, p=weight) + sampled_index = ( + sampled_i // dg_prb.shape[1], + sampled_i % dg_prb.shape[1], + ) + c_grasp = np.zeros(dg_prb.shape) + c_grasp[sampled_index] = 255 + c_grasp = scipy.ndimage.filters.gaussian_filter( + c_grasp, sigma=20) + c_grasp = c_grasp / c_grasp.max() + c_mask = c_grasp > self.sampling_thresh + if len(c_masks) > 0: + if not np.any(np.logical_and(c_mask, c_masks[0])): + c_masks.append(c_mask) + break + else: + c_masks.append(c_mask) + break + if i == trial_num - 1: + dual_giveup = True + except Exception: + dual_giveup = True + if len(c_masks) == 2 and dual_giveup is False: + sample_mask = np.logical_or(c_masks[0], c_masks[1]) + sample_mask = sample_mask.astype(np.uint8) * 255 + else: + sample_mask = np.zeros(dg_prb.shape, dtype=np.uint8) + + return sample_mask + + def _find_occ_top(self, target_ins_id, ins_label, label): + checked_ids = set() + checked_ids.add(target_ins_id) + vis_msk = ins_label[target_ins_id] == 1 + vis_rto = vis_msk.sum() / (ins_label[target_ins_id] > 0).sum() + if vis_rto > self.vis_thresh: + rospy.loginfo( + '{}_{} is not occluded but graspable!' + .format(self.label_names[label[target_ins_id]], target_ins_id)) + return target_ins_id + ret_ins_id = None + for ins_id, ins_lbl in enumerate(ins_label): + if ins_id in checked_ids: + continue + else: + occ_msk = ins_label[target_ins_id] == 2 + occ_msk_by_this = np.logical_and(ins_lbl == 1, occ_msk) + occ_rto = occ_msk_by_this.sum() / occ_msk.sum() + if occ_rto > 0.1: + rospy.loginfo( + '{}_{} is occluded by {}_{}' + .format(self.label_names[label[target_ins_id]], + target_ins_id, + self.label_names[label[ins_id]], ins_id)) + ret_ins_id = self._find_occ_top_step( + ins_id, ins_label, label, checked_ids) + if ret_ins_id is not None: + break + if ret_ins_id is None: + return target_ins_id + else: + return ret_ins_id + + def _find_occ_top_step(self, target_ins_id, ins_label, label, checked_ids): + checked_ids.add(target_ins_id) + vis_msk = ins_label[target_ins_id] == 1 + vis_rto = vis_msk.sum() / (ins_label[target_ins_id] > 0).sum() + if vis_rto > self.vis_thresh: + rospy.loginfo( + '{}_{} is not occluded but graspable!' + .format(self.label_names[label[target_ins_id]], target_ins_id)) + return target_ins_id + ret_ins_id = None + for ins_id, ins_lbl in enumerate(ins_label): + if ins_id in checked_ids: + continue + else: + occ_msk = ins_label[target_ins_id] == 2 + occ_msk_by_this = np.logical_and(ins_lbl == 1, occ_msk) + occ_rto = occ_msk_by_this.sum() / occ_msk.sum() + if occ_rto > 0.1: + rospy.loginfo( + '{}_{} is occluded by {}_{}' + .format(self.label_names[label[target_ins_id]], + target_ins_id, + self.label_names[label[ins_id]], ins_id)) + ret_ins_id = self._find_occ_top_step( + ins_id, ins_label, label, checked_ids) + if ret_ins_id is not None: + break + return ret_ins_id + + def _get_another(self, req): + grasp_style = req.style + label = req.label + self.giveup_ins_ids[grasp_style].append(label) + response = GetAnotherResponse() + response.success = True + return response + + def _reset(self, req): + self.giveup_ins_ids = { + 'single': [], + 'dual': [], + } + response = TriggerResponse() + response.success = True + return response + + def _dyn_callback(self, config, level): + self.score_thresh = config.score_thresh + self.grasp_thresh = config.grasp_thresh + self.nms_thresh = config.nms_thresh + self.vis_thresh = config.vis_thresh + self.sampling_thresh = config.sampling_thresh + self.grasping_way = config.grasping_way + return config + + +if __name__ == '__main__': + rospy.init_node('dualarm_occluded_grasp_instance_segmentation') + node = DualarmOccludedGraspInstanceSegmentation() + rospy.spin() diff --git a/demos/selective_dualarm_grasping/package.xml b/demos/selective_dualarm_grasping/package.xml new file mode 100644 index 000000000..d1b26a5eb --- /dev/null +++ b/demos/selective_dualarm_grasping/package.xml @@ -0,0 +1,32 @@ + + + dualarm_grasping + 0.0.0 + The dualarm_grasping package + + Shingo Kitagawa + Shingo Kitagawa + MIT + + catkin + dynamic_reconfigure + message_generation + jsk_recognition_msgs + std_msgs + + message_runtime + python-chainer-pip + python-fcn-pip + jsk_2015_05_baxter_apc + jsk_2016_01_baxter_apc + jsk_arc2017_baxter + jsk_arc2017_common + jsk_data + jsk_recognition_msgs + roseus_smach + std_msgs + xacro + + + + diff --git a/demos/selective_dualarm_grasping/python/dualarm_grasping/__init__.py b/demos/selective_dualarm_grasping/python/dualarm_grasping/__init__.py new file mode 100644 index 000000000..4d4a78981 --- /dev/null +++ b/demos/selective_dualarm_grasping/python/dualarm_grasping/__init__.py @@ -0,0 +1 @@ +from dualarm_grasping import models # NOQA diff --git a/demos/selective_dualarm_grasping/rvizconfig/baxter/default.rviz b/demos/selective_dualarm_grasping/rvizconfig/baxter/default.rviz new file mode 100644 index 000000000..e6f3df7e2 --- /dev/null +++ b/demos/selective_dualarm_grasping/rvizconfig/baxter/default.rviz @@ -0,0 +1,1202 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1/Scene Robot1 + Splitter Ratio: 0.658333 + Tree Height: 775 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LeftToteCloud +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LeftToteCloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /left_hand_camera/extract_indices_tote/output + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: RightToteCloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /right_hand_camera/extract_indices_tote/output + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: jsk_rviz_plugin/BoundingBoxArray + Enabled: true + Name: LeftHandGraspBbox + Topic: /left_hand_camera/cluster_indices_decomposer/boxes + Unreliable: false + Value: true + alpha: 0.8 + color: 25; 255; 0 + coloring: Flat color + line width: 0.005 + only edge: false + show coords: false + - Class: jsk_rviz_plugin/BoundingBoxArray + Enabled: true + Name: RightHandGraspBBox + Topic: /right_hand_camera/cluster_indices_decomposer/boxes + Unreliable: false + Value: true + alpha: 0.8 + color: 0; 170; 127 + coloring: Flat color + line width: 0.005 + only edge: false + show coords: false + - Class: rviz/InteractiveMarkers + Enable Transparency: true + Enabled: false + Name: InteractiveMarkers + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Update Topic: /simple_marker/update + Value: false + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + collision_head_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + collision_head_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + display: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + dummyhead1: + Alpha: 1 + Show Axes: false + Show Trail: false + head: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + left_arm_itb: + Alpha: 1 + Show Axes: false + Show Trail: false + left_arm_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + left_gripper_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_l_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_l_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_pad_with_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_palm_endpoint: + Alpha: 1 + Show Axes: false + Show Trail: false + left_gripper_r_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_r_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_tube: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand_accelerometer: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand_camera_axis: + Alpha: 1 + Show Axes: false + Show Trail: false + left_hand_range: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_lower_elbow: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_lower_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_lower_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_torso_itb: + Alpha: 1 + Show Axes: false + Show Trail: false + left_upper_elbow: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_upper_elbow_visual: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_upper_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_upper_forearm_visual: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_upper_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pedestal: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_arm_itb: + Alpha: 1 + Show Axes: false + Show Trail: false + right_arm_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + right_gripper_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_l_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_l_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_pad_with_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_palm_endpoint: + Alpha: 1 + Show Axes: false + Show Trail: false + right_gripper_r_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_r_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_tube: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand_accelerometer: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand_camera_axis: + Alpha: 1 + Show Axes: false + Show Trail: false + right_hand_range: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_lower_elbow: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_lower_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_lower_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_torso_itb: + Alpha: 1 + Show Axes: false + Show Trail: false + right_upper_elbow: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_upper_elbow_visual: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_upper_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_upper_forearm_visual: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_upper_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + screen: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sonar_ring: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/Image + Enabled: true + Image Topic: /left_hand_camera/label_image_decomposer/output/label_viz + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: LeftLabelImage + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /right_hand_camera/label_image_decomposer/output/label_viz + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: RightLabelImage + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Goal_Tolerance: 0 + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Constraint_Aware_IK: true + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + collision_head_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + collision_head_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + display: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + dummyhead1: + Alpha: 1 + Show Axes: false + Show Trail: false + head: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + left_arm_itb: + Alpha: 1 + Show Axes: false + Show Trail: false + left_arm_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + left_gripper_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_l_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_l_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_pad_with_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_palm_endpoint: + Alpha: 1 + Show Axes: false + Show Trail: false + left_gripper_r_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_r_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_tube: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand_accelerometer: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand_camera_axis: + Alpha: 1 + Show Axes: false + Show Trail: false + left_hand_range: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_lower_elbow: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_lower_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_lower_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_torso_itb: + Alpha: 1 + Show Axes: false + Show Trail: false + left_upper_elbow: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_upper_elbow_visual: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_upper_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_upper_forearm_visual: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_upper_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pedestal: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_arm_itb: + Alpha: 1 + Show Axes: false + Show Trail: false + right_arm_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + right_gripper_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_l_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_l_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_pad_with_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_palm_endpoint: + Alpha: 1 + Show Axes: false + Show Trail: false + right_gripper_r_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_r_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_tube: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand_accelerometer: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand_camera_axis: + Alpha: 1 + Show Axes: false + Show Trail: false + right_hand_range: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_lower_elbow: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_lower_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_lower_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_torso_itb: + Alpha: 1 + Show Axes: false + Show Trail: false + right_upper_elbow: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_upper_elbow_visual: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_upper_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_upper_forearm_visual: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_upper_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + screen: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sonar_ring: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: true + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /move_group/display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.08 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: both_arms + Query Goal State: false + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: /move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.2 + Scene Color: 50; 230; 50 + Scene Display Time: 0.2 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + collision_head_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + collision_head_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + display: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + dummyhead1: + Alpha: 1 + Show Axes: false + Show Trail: false + head: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + left_arm_itb: + Alpha: 1 + Show Axes: false + Show Trail: false + left_arm_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + left_gripper_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_l_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_l_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_pad_with_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_palm_endpoint: + Alpha: 1 + Show Axes: false + Show Trail: false + left_gripper_r_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_r_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_tube: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand_accelerometer: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand_camera_axis: + Alpha: 1 + Show Axes: false + Show Trail: false + left_hand_range: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_lower_elbow: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_lower_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_lower_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_torso_itb: + Alpha: 1 + Show Axes: false + Show Trail: false + left_upper_elbow: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_upper_elbow_visual: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_upper_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_upper_forearm_visual: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_upper_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pedestal: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_arm_itb: + Alpha: 1 + Show Axes: false + Show Trail: false + right_arm_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + right_gripper_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_l_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_l_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_pad_with_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_palm_endpoint: + Alpha: 1 + Show Axes: false + Show Trail: false + right_gripper_r_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_r_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_tube: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand_accelerometer: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand_camera_axis: + Alpha: 1 + Show Axes: false + Show Trail: false + right_hand_range: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_lower_elbow: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_lower_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_lower_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_torso_itb: + Alpha: 1 + Show Axes: false + Show Trail: false + right_upper_elbow: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_upper_elbow_visual: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_upper_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_upper_forearm_visual: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_upper_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + screen: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sonar_ring: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 3.57766 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.603229 + Y: 0.103062 + Z: -0.109104 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.799797 + Target Frame: + Value: Orbit (rviz) + Yaw: 6.27357 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1056 + Hide Left Dock: false + Hide Right Dock: false + LeftLabelImage: + collapsed: false + MotionPlanning: + collapsed: false + MotionPlanning - Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000002a200000396fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000120020002d00200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000001e7000001d7000001cc00fffffffb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c00690064006500720000000000ffffffff0000004a00ffffff000000010000021500000396fc0200000005fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002ab000000b000fffffffb0000001c004c006500660074004c006100620065006c0049006d0061006700650100000028000001d60000001600fffffffb0000001e00520069006700680074004c006100620065006c0049006d0061006700650100000204000001ba0000001600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009bf0000003efc0100000002fb0000000800540069006d00650100000000000009bf000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000004fc0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + RightLabelImage: + collapsed: false + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2495 + X: 65 + Y: 24 diff --git a/demos/selective_dualarm_grasping/rvizconfig/baxter/default_occluded.rviz b/demos/selective_dualarm_grasping/rvizconfig/baxter/default_occluded.rviz new file mode 100644 index 000000000..ceece1294 --- /dev/null +++ b/demos/selective_dualarm_grasping/rvizconfig/baxter/default_occluded.rviz @@ -0,0 +1,1230 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1/Scene Robot1 + Splitter Ratio: 0.658333 + Tree Height: 775 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LeftToteCloud +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LeftToteCloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /left_hand_camera/extract_indices_tote/output + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: RightToteCloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /right_hand_camera/extract_indices_tote/output + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: jsk_rviz_plugin/BoundingBoxArray + Enabled: true + Name: LeftHandGraspBbox + Topic: /left_hand_camera/cluster_indices_decomposer/boxes + Unreliable: false + Value: true + alpha: 0.8 + color: 25; 255; 0 + coloring: Flat color + line width: 0.005 + only edge: false + show coords: false + - Class: jsk_rviz_plugin/BoundingBoxArray + Enabled: true + Name: RightHandGraspBBox + Topic: /right_hand_camera/cluster_indices_decomposer/boxes + Unreliable: false + Value: true + alpha: 0.8 + color: 0; 170; 127 + coloring: Flat color + line width: 0.005 + only edge: false + show coords: false + - Class: rviz/InteractiveMarkers + Enable Transparency: true + Enabled: false + Name: InteractiveMarkers + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Update Topic: /simple_marker/update + Value: false + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + collision_head_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + collision_head_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + display: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + dummyhead1: + Alpha: 1 + Show Axes: false + Show Trail: false + head: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + left_arm_itb: + Alpha: 1 + Show Axes: false + Show Trail: false + left_arm_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + left_gripper_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_l_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_l_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_pad_with_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_palm_endpoint: + Alpha: 1 + Show Axes: false + Show Trail: false + left_gripper_r_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_r_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_tube: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand_accelerometer: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand_camera_axis: + Alpha: 1 + Show Axes: false + Show Trail: false + left_hand_range: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_lower_elbow: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_lower_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_lower_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_torso_itb: + Alpha: 1 + Show Axes: false + Show Trail: false + left_upper_elbow: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_upper_elbow_visual: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_upper_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_upper_forearm_visual: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_upper_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pedestal: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_arm_itb: + Alpha: 1 + Show Axes: false + Show Trail: false + right_arm_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + right_gripper_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_l_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_l_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_pad_with_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_palm_endpoint: + Alpha: 1 + Show Axes: false + Show Trail: false + right_gripper_r_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_r_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_tube: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand_accelerometer: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand_camera_axis: + Alpha: 1 + Show Axes: false + Show Trail: false + right_hand_range: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_lower_elbow: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_lower_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_lower_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_torso_itb: + Alpha: 1 + Show Axes: false + Show Trail: false + right_upper_elbow: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_upper_elbow_visual: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_upper_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_upper_forearm_visual: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_upper_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + screen: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sonar_ring: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/Image + Enabled: true + Image Topic: /left_hand_camera/dualarm_grasp_segmentation/debug/vis_output + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: LeftVisImage + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /left_hand_camera/tile_image/output + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: LeftTileImage + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /right_hand_camera/dualarm_grasp_segmentation/debug/vis_output + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: RightVisImage + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /right_hand_camera/tile_image/output + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: RightTileImage + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Goal_Tolerance: 0 + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Constraint_Aware_IK: true + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + collision_head_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + collision_head_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + display: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + dummyhead1: + Alpha: 1 + Show Axes: false + Show Trail: false + head: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + left_arm_itb: + Alpha: 1 + Show Axes: false + Show Trail: false + left_arm_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + left_gripper_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_l_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_l_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_pad_with_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_palm_endpoint: + Alpha: 1 + Show Axes: false + Show Trail: false + left_gripper_r_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_r_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_tube: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand_accelerometer: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand_camera_axis: + Alpha: 1 + Show Axes: false + Show Trail: false + left_hand_range: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_lower_elbow: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_lower_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_lower_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_torso_itb: + Alpha: 1 + Show Axes: false + Show Trail: false + left_upper_elbow: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_upper_elbow_visual: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_upper_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_upper_forearm_visual: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_upper_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pedestal: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_arm_itb: + Alpha: 1 + Show Axes: false + Show Trail: false + right_arm_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + right_gripper_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_l_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_l_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_pad_with_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_palm_endpoint: + Alpha: 1 + Show Axes: false + Show Trail: false + right_gripper_r_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_r_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_tube: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand_accelerometer: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand_camera_axis: + Alpha: 1 + Show Axes: false + Show Trail: false + right_hand_range: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_lower_elbow: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_lower_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_lower_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_torso_itb: + Alpha: 1 + Show Axes: false + Show Trail: false + right_upper_elbow: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_upper_elbow_visual: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_upper_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_upper_forearm_visual: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_upper_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + screen: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sonar_ring: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: true + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /move_group/display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.08 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: both_arms + Query Goal State: false + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: /move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.2 + Scene Color: 50; 230; 50 + Scene Display Time: 0.2 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + collision_head_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + collision_head_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + display: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + dummyhead1: + Alpha: 1 + Show Axes: false + Show Trail: false + head: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + left_arm_itb: + Alpha: 1 + Show Axes: false + Show Trail: false + left_arm_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + left_gripper_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_l_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_l_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_pad_with_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_palm_endpoint: + Alpha: 1 + Show Axes: false + Show Trail: false + left_gripper_r_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_r_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_tube: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand_accelerometer: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand_camera_axis: + Alpha: 1 + Show Axes: false + Show Trail: false + left_hand_range: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_lower_elbow: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_lower_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_lower_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_torso_itb: + Alpha: 1 + Show Axes: false + Show Trail: false + left_upper_elbow: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_upper_elbow_visual: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_upper_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_upper_forearm_visual: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_upper_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pedestal: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_arm_itb: + Alpha: 1 + Show Axes: false + Show Trail: false + right_arm_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + right_gripper_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_l_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_l_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_pad_with_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_palm_endpoint: + Alpha: 1 + Show Axes: false + Show Trail: false + right_gripper_r_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_r_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_tube: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand_accelerometer: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand_camera_axis: + Alpha: 1 + Show Axes: false + Show Trail: false + right_hand_range: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_lower_elbow: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_lower_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_lower_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_torso_itb: + Alpha: 1 + Show Axes: false + Show Trail: false + right_upper_elbow: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_upper_elbow_visual: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_upper_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_upper_forearm_visual: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_upper_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + screen: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sonar_ring: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 3.14834 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.603229 + Y: 0.103062 + Z: -0.109104 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 1.0598 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.00538481 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1056 + Hide Left Dock: false + Hide Right Dock: false + LeftTileImage: + collapsed: false + LeftVisImage: + collapsed: false + MotionPlanning: + collapsed: false + MotionPlanning - Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000002a200000396fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000120020002d00200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000001e7000001d7000001cc00fffffffb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c00690064006500720000000000ffffffff0000004a00ffffff00000001000002ba00000396fc0200000007fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002ab000000b000fffffffb00000018004c0065006600740056006900730049006d00610067006501000000280000009e0000001600fffffffb0000001a004c00650066007400540069006c00650049006d00610067006501000000cc0000011d0000001600fffffffb0000001a005200690067006800740056006900730049006d00610067006501000001ef0000009d0000001600fffffffb0000001c0052006900670068007400540069006c00650049006d00610067006501000002920000012c0000001600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009bf0000003efc0100000002fb0000000800540069006d00650100000000000009bf000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000004570000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + RightTileImage: + collapsed: false + RightVisImage: + collapsed: false + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2495 + X: 65 + Y: 24 diff --git a/demos/selective_dualarm_grasping/rvizconfig/baxter/default_occlusion_dataset.rviz b/demos/selective_dualarm_grasping/rvizconfig/baxter/default_occlusion_dataset.rviz new file mode 100644 index 000000000..601cf4d77 --- /dev/null +++ b/demos/selective_dualarm_grasping/rvizconfig/baxter/default_occlusion_dataset.rviz @@ -0,0 +1,492 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.658333 + Tree Height: 775 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: RightToteCloud +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: RightToteCloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /extract_indices_tote/output + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/InteractiveMarkers + Enable Transparency: true + Enabled: false + Name: InteractiveMarkers + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Update Topic: /simple_marker/update + Value: false + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + collision_head_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + collision_head_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + display: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + dummyhead1: + Alpha: 1 + Show Axes: false + Show Trail: false + head: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + left_arm_itb: + Alpha: 1 + Show Axes: false + Show Trail: false + left_arm_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + left_gripper_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_l_finger_a: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_l_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_l_finger_bc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_pad_with_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_palm_endpoint: + Alpha: 1 + Show Axes: false + Show Trail: false + left_gripper_r_finger_a: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_r_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_r_finger_bc: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_gripper_tube: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand_accelerometer: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand_camera_axis: + Alpha: 1 + Show Axes: false + Show Trail: false + left_hand_range: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_lower_elbow: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_lower_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_lower_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_torso_itb: + Alpha: 1 + Show Axes: false + Show Trail: false + left_upper_elbow: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_upper_elbow_visual: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_upper_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_upper_forearm_visual: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_upper_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pedestal: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_arm_itb: + Alpha: 1 + Show Axes: false + Show Trail: false + right_arm_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + right_gripper_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_l_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_l_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_pad_with_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_palm_endpoint: + Alpha: 1 + Show Axes: false + Show Trail: false + right_gripper_r_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_r_finger_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_gripper_tube: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand_accelerometer: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand_camera_axis: + Alpha: 1 + Show Axes: false + Show Trail: false + right_hand_range: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_lower_elbow: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_lower_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_lower_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_torso_itb: + Alpha: 1 + Show Axes: false + Show Trail: false + right_upper_elbow: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_upper_elbow_visual: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_upper_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_upper_forearm_visual: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_upper_shoulder: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + screen: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sonar_ring: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/Image + Enabled: true + Image Topic: /right_hand_camera/left/rgb/image_rect_color + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: RGB Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /apply_mask/output + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Masked RGB Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 3.14834 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.603229 + Y: 0.103062 + Z: -0.109104 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 1.0598 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.00538481 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1056 + Hide Left Dock: false + Hide Right Dock: false + Masked RGB Image: + collapsed: false + QMainWindow State: 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 + RGB Image: + collapsed: false + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2495 + X: 65 + Y: 24 diff --git a/demos/selective_dualarm_grasping/rvizconfig/pr2/default_occluded.rviz b/demos/selective_dualarm_grasping/rvizconfig/pr2/default_occluded.rviz new file mode 100644 index 000000000..c74120b6d --- /dev/null +++ b/demos/selective_dualarm_grasping/rvizconfig/pr2/default_occluded.rviz @@ -0,0 +1,1629 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /MotionPlanning1/Scene Robot1 + Splitter Ratio: 0.658333 + Tree Height: 775 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: HeadTileImage +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: HeadCloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /kinect_head_remote/extract_indices_tote/output + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: jsk_rviz_plugin/BoundingBoxArray + Enabled: true + Name: HeadGraspBbox + Topic: /kinect_head_remote/cluster_indices_decomposer/boxes + Unreliable: false + Value: true + alpha: 0.8 + color: 25; 255; 0 + coloring: Flat color + line width: 0.005 + only edge: false + show coords: false + - Class: rviz/InteractiveMarkers + Enable Transparency: true + Enabled: false + Name: InteractiveMarkers + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Update Topic: /simple_marker/update + Value: false + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_bellow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + double_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_chain_cb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + head_mount_kinect_ir_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_ir_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_mount_kinect_rgb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_rgb_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_plate_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + high_def_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + high_def_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + l_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_cam_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_cam_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_tip_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + l_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_led_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + l_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_motor_screw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + l_gripper_motor_slider_link: + Alpha: 1 + Show Axes: false + Show Trail: false + l_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_tool_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + l_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_torso_lift_side_plate_link: + Alpha: 1 + Show Axes: false + Show Trail: false + l_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + laser_tilt_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_arm_chain_cb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + narrow_stereo_l_stereo_camera_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + narrow_stereo_l_stereo_camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + narrow_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + narrow_stereo_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + narrow_stereo_r_stereo_camera_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + narrow_stereo_r_stereo_camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + projector_wg6802418_child_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + projector_wg6802418_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_cam_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_cam_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_tip_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_led_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_motor_screw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + r_gripper_motor_slider_link: + Alpha: 1 + Show Axes: false + Show Trail: false + r_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_tool_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_torso_lift_side_plate_link: + Alpha: 1 + Show Axes: false + Show Trail: false + r_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_arm_chain_cb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + sensor_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_motor_screw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_stereo_l_stereo_camera_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_stereo_l_stereo_camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_stereo_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_stereo_r_stereo_camera_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_stereo_r_stereo_camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/Image + Enabled: true + Image Topic: /kinect_head_remote/dualarm_grasp_segmentation/debug/vis_output + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: HeadVisImage + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /kinect_head_remote/tile_image/output + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: HeadTileImage + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Goal_Tolerance: 0 + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Constraint_Aware_IK: true + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_bellow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + double_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_chain_cb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + head_mount_kinect_ir_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_ir_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_mount_kinect_rgb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_rgb_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_plate_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + high_def_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + high_def_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + l_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_cam_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_cam_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_tip_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + l_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_led_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + l_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_motor_screw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + l_gripper_motor_slider_link: + Alpha: 1 + Show Axes: false + Show Trail: false + l_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_tool_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + l_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_torso_lift_side_plate_link: + Alpha: 1 + Show Axes: false + Show Trail: false + l_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + laser_tilt_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_arm_chain_cb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + narrow_stereo_l_stereo_camera_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + narrow_stereo_l_stereo_camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + narrow_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + narrow_stereo_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + narrow_stereo_r_stereo_camera_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + narrow_stereo_r_stereo_camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + projector_wg6802418_child_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + projector_wg6802418_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_cam_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_cam_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_tip_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_led_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_motor_screw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + r_gripper_motor_slider_link: + Alpha: 1 + Show Axes: false + Show Trail: false + r_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_tool_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_torso_lift_side_plate_link: + Alpha: 1 + Show Axes: false + Show Trail: false + r_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_arm_chain_cb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + sensor_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_motor_screw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_stereo_l_stereo_camera_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_stereo_l_stereo_camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_stereo_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_stereo_r_stereo_camera_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_stereo_r_stereo_camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: true + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /move_group/display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.08 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: "" + Query Goal State: false + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: /move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.2 + Scene Color: 50; 230; 50 + Scene Display Time: 0.2 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_bellow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + double_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_chain_cb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + head_mount_kinect_ir_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_ir_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_mount_kinect_rgb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_rgb_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_plate_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + high_def_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + high_def_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + l_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_cam_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_cam_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_tip_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + l_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_led_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + l_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_motor_screw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + l_gripper_motor_slider_link: + Alpha: 1 + Show Axes: false + Show Trail: false + l_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_tool_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + l_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_torso_lift_side_plate_link: + Alpha: 1 + Show Axes: false + Show Trail: false + l_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + laser_tilt_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_arm_chain_cb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + narrow_stereo_l_stereo_camera_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + narrow_stereo_l_stereo_camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + narrow_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + narrow_stereo_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + narrow_stereo_r_stereo_camera_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + narrow_stereo_r_stereo_camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + projector_wg6802418_child_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + projector_wg6802418_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_cam_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_cam_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_tip_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_led_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_motor_screw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + r_gripper_motor_slider_link: + Alpha: 1 + Show Axes: false + Show Trail: false + r_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_tool_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_torso_lift_side_plate_link: + Alpha: 1 + Show Axes: false + Show Trail: false + r_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_arm_chain_cb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + sensor_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_motor_screw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_stereo_l_stereo_camera_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_stereo_l_stereo_camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_stereo_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_stereo_r_stereo_camera_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_stereo_r_stereo_camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 3.19593 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.643058 + Y: -0.112129 + Z: -0.0226482 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.324796 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.47538 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + HeadTileImage: + collapsed: false + HeadVisImage: + collapsed: false + Height: 1056 + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Slider: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2495 + X: 65 + Y: 24 diff --git a/demos/selective_dualarm_grasping/sample/sample_dualarm_grasp_segmentation.launch b/demos/selective_dualarm_grasping/sample/sample_dualarm_grasp_segmentation.launch new file mode 100644 index 000000000..760d6d7b9 --- /dev/null +++ b/demos/selective_dualarm_grasping/sample/sample_dualarm_grasp_segmentation.launch @@ -0,0 +1,64 @@ + + + + + + + + file_name: $(arg filename) + publish_info: false + encoding: bgr8 + + + + + + + offset_x: 0 + offset_y: 0 + width: 640 + height: 480 + + + + + + + + gpu: 0 + model_file: $(find dualarm_grasping)/models/dualarm_grasp/self_anno/201802220906_iter00200000.npz + score_thresh: 0.5 + use_mask: true + approximate_sync: true + queue_size: 100 + + + + + + + + gpu: 0 + model_file: $(find dualarm_grasping)/models/dualarm_grasp/self_anno/201802220906_iter00200000.npz + score_thresh: 0.5 + use_mask: false + approximate_sync: true + queue_size: 100 + + + + + + + + + + + diff --git a/demos/selective_dualarm_grasping/scripts/get_objects_order.py b/demos/selective_dualarm_grasping/scripts/get_objects_order.py new file mode 100755 index 000000000..f512ae118 --- /dev/null +++ b/demos/selective_dualarm_grasping/scripts/get_objects_order.py @@ -0,0 +1,14 @@ +import os.path as osp +import random +import yaml + + +filepath = osp.dirname(osp.realpath(__file__)) +yamlpath = osp.join(filepath, '../yaml/dualarm_grasping_label_names.yaml') +with open(yamlpath, 'r') as f: + object_names = yaml.load(f)['label_names'][1:] + +order = range(len(object_names)) +order = random.sample(order, len(order)) +for i, ordr in enumerate(order): + print('{}: {}'.format(i, object_names[ordr])) diff --git a/demos/selective_dualarm_grasping/scripts/install_models.py b/demos/selective_dualarm_grasping/scripts/install_models.py new file mode 100755 index 000000000..62dd44961 --- /dev/null +++ b/demos/selective_dualarm_grasping/scripts/install_models.py @@ -0,0 +1,188 @@ +#!/usr/bin/env python + +import argparse +import multiprocessing + +import jsk_data + + +def download_data(*args, **kwargs): + p = multiprocessing.Process( + target=jsk_data.download_data, + args=args, + kwargs=kwargs) + p.start() + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument('-v', '--verbose', dest='quiet', action='store_false') + args = parser.parse_args() + quiet = args.quiet + + PKG = 'dualarm_grasping' + + download_data( + pkg_name=PKG, + path='models/dualarm_grasp/human_anno/20171129_iter00200000.npz', + url='https://drive.google.com/uc?id=1EDoJMJukap71UrCg1Sm29vuDDqhWkqwd', + md5='a7cbb66a2f37bcb04955286f6d92662c', + quiet=quiet + ) + download_data( + pkg_name=PKG, + path='models/dualarm_grasp/self_anno/20180202_iter00200000.npz', + url='https://drive.google.com/uc?id=1eOBvwALy-R-SBWKL6GYNTGiGzlBeBsV4', + md5='e246250eaf51c45b90c5f8fed9d29286', + quiet=quiet + ) + + download_data( + pkg_name=PKG, + path='models/dualarm_grasp/self_anno/201802220906_iter00200000.npz', + url='https://drive.google.com/uc?id=1dKXj2TzryyRoW7aRHZJeG6Gkl62GGB-r', + md5='032abfbdc26bcad7b4d933c202e3e91b', + quiet=quiet + ) + + download_data( + pkg_name=PKG, + path='models/dualarm_grasp/human_anno/201802220907_iter00200000.npz', + url='https://drive.google.com/uc?id=1Fi3-adAMq_FL12KSmfLfGE-3SbxGeLip', + md5='35904d072c1174d110b5b6d7c9a65659', + quiet=quiet + ) + + # trained with 1st sampling data + download_data( + pkg_name=PKG, + path='models/dualarm_grasp/self_anno/201802241311_iter00012000.npz', + url='https://drive.google.com/uc?id=1H5yQ8zGglaoIdZ4f1CS8yAAY--i5Vj44', + md5='92c0a37021004bf5ad52e42a793e4a7a', + quiet=quiet + ) + + # trained with 2nd sampling data + download_data( + pkg_name=PKG, + path='models/dualarm_grasp/self_anno/201802261200_iter00012000.npz', + url='https://drive.google.com/uc?id=1yGdgp6Bvw2Omn57Cd6517MVhox-0RTPL', + md5='bd6daa72a15801e382202c591c9ad8c1', + quiet=quiet + ) + + # trained with 2nd sampling data + download_data( + pkg_name=PKG, + path='models/dualarm_grasp/self_anno/201802261200_iter00048000.npz', + url='https://drive.google.com/uc?id=1N1YvzSEXbLgTxDZMNd-el_3fbryx253j', + md5='bb6b14f261dbbe0f3b4011ca626d3de3', + quiet=quiet + ) + + # trained with 2nd sampling data + download_data( + pkg_name=PKG, + path='models/dualarm_grasp/self_anno/201802261200_iter00200000.npz', + url='https://drive.google.com/uc?id=1xIf4lp1T4lpVadIRmfiLdc76clKYIBG2', + md5='d80aa45ab04c6fd66e15ea819699296b', + quiet=quiet + ) + + # trained with 2nd sampling data + download_data( + pkg_name=PKG, + path='models/dualarm_grasp/self_anno/201802261705_iter00012000.npz', + url='https://drive.google.com/uc?id=1UzABg0E3-aoO73wJHQSEf4VH4fAoRSp_', + md5='6933e9341ff8eb16b33ed8b1c36e690b', + quiet=quiet + ) + + # mask rcnn + # trained with synthesized data + download_data( + pkg_name=PKG, + path='models/dualarm_occluded_grasp/self_anno/20181127_model_iter_54480.npz', # NOQA + url='https://drive.google.com/uc?id=1ITLgC8TMHRljfq76hacQJ0FpMM3aSPWV', + md5='53ed87ab425b65fd02bd47109dc505f9', + quiet=quiet + ) + download_data( + pkg_name=PKG, + path='models/dualarm_occluded_grasp/self_anno/20181127_params.yaml', # NOQA + url='https://drive.google.com/uc?id=1REdW1rRAKtSCzyVTbckvs7yzjO-YsD1n', + md5='e3fd1169da238a96708cbfea8cc3c1fa', + quiet=quiet + ) + + # # mask rcnn + # # trained with 1st sampling + # download_data( + # pkg_name=PKG, + # path='models/dualarm_occluded_grasp/self_anno/20181218_model_iter_26568.npz', # NOQA + # url='https://drive.google.com/uc?id=1hhSi8RUXNq91y7UbEYrXlmvGJLc9uakJ', + # md5='7d2b9dfc605c36cb3de1047ab20197fe', + # quiet=quiet + # ) + # download_data( + # pkg_name=PKG, + # path='models/dualarm_occluded_grasp/self_anno/20181218_params.yaml', # NOQA + # url='https://drive.google.com/uc?id=12bSetNJhBTYCdIJf5GNbP2RHKr0nnM7r', + # md5='68682e89f414865b5d0e9557995610a8', + # quiet=quiet + # ) + + # mask rcnn + # trained with 1st sampling + download_data( + pkg_name=PKG, + path='models/dualarm_occluded_grasp/self_anno/20181226_model_iter_13284.npz', # NOQA + url='https://drive.google.com/uc?id=1jhzj5D0iAJgBwRLtQCZTLYk6rl-MbGuc', + md5='05514904be0c18d815ff98e7cd8e0b1b', + quiet=quiet + ) + download_data( + pkg_name=PKG, + path='models/dualarm_occluded_grasp/self_anno/20181226_params.yaml', # NOQA + url='https://drive.google.com/uc?id=15CLY2OnFnk_LvDPKLlx1uqofqxmvmhCm', + md5='90feeabaa144085c712b223d517b930f', + quiet=quiet + ) + + # # mask rcnn + # # trained with 2nd sampling + # download_data( + # pkg_name=PKG, + # path='models/dualarm_occluded_grasp/self_anno/20181226_213602_model_iter_13677.npz', # NOQA + # url='https://drive.google.com/uc?id=1trtL2dmBt36FLw1m5efKwUGk9EapiIxa', + # md5='bb01a0704ab8733a135e7b492231d423', + # quiet=quiet + # ) + # download_data( + # pkg_name=PKG, + # path='models/dualarm_occluded_grasp/self_anno/20181226_213602_params.yaml', # NOQA + # url='https://drive.google.com/uc?id=1FQousbTcIQtjv-c0Svl80in8F5E4jzPF', + # md5='ff03abf9339612d2dba7fdcfe8743b27', + # quiet=quiet + # ) + + # mask rcnn + # trained with 2nd sampling + download_data( + pkg_name=PKG, + path='models/dualarm_occluded_grasp/self_anno/20181227_model_iter_4559.npz', # NOQA + url='https://drive.google.com/uc?id=1L_kMSDR0yryojL4iHvXUXtR0PKgQ8obl', + md5='3951b47d0c2441b71a9069db3a1a1155', + quiet=quiet + ) + download_data( + pkg_name=PKG, + path='models/dualarm_occluded_grasp/self_anno/20181227_params.yaml', # NOQA + url='https://drive.google.com/uc?id=1uMtfdif6HXQdVVvGEBlStxqzVMmM4_u6', + md5='23947a4dfc850a10679b845720e56f41', + quiet=quiet + ) + + +if __name__ == '__main__': + main() diff --git a/demos/selective_dualarm_grasping/setup.py b/demos/selective_dualarm_grasping/setup.py new file mode 100644 index 000000000..efe67b64f --- /dev/null +++ b/demos/selective_dualarm_grasping/setup.py @@ -0,0 +1,14 @@ +#!/usr/bin/env python + +from distutils.core import setup + +from catkin_pkg.python_setup import generate_distutils_setup +from setuptools import find_packages + + +d = generate_distutils_setup( + packages=find_packages('python'), + package_dir={'': 'python'}, +) + +setup(**d) diff --git a/demos/selective_dualarm_grasping/srv/GetAnother.srv b/demos/selective_dualarm_grasping/srv/GetAnother.srv new file mode 100644 index 000000000..4ea9f0672 --- /dev/null +++ b/demos/selective_dualarm_grasping/srv/GetAnother.srv @@ -0,0 +1,4 @@ +string style +int8 label +--- +bool success diff --git a/demos/selective_dualarm_grasping/test_data/00.jpg b/demos/selective_dualarm_grasping/test_data/00.jpg new file mode 100644 index 000000000..b3b78bb20 Binary files /dev/null and b/demos/selective_dualarm_grasping/test_data/00.jpg differ diff --git a/demos/selective_dualarm_grasping/test_data/01.jpg b/demos/selective_dualarm_grasping/test_data/01.jpg new file mode 100644 index 000000000..8c25b5713 Binary files /dev/null and b/demos/selective_dualarm_grasping/test_data/01.jpg differ diff --git a/demos/selective_dualarm_grasping/test_data/02.jpg b/demos/selective_dualarm_grasping/test_data/02.jpg new file mode 100644 index 000000000..85983e0be Binary files /dev/null and b/demos/selective_dualarm_grasping/test_data/02.jpg differ diff --git a/demos/selective_dualarm_grasping/test_data/03.jpg b/demos/selective_dualarm_grasping/test_data/03.jpg new file mode 100644 index 000000000..3b4bf5fed Binary files /dev/null and b/demos/selective_dualarm_grasping/test_data/03.jpg differ diff --git a/demos/selective_dualarm_grasping/test_data/04.jpg b/demos/selective_dualarm_grasping/test_data/04.jpg new file mode 100644 index 000000000..c1f2f349b Binary files /dev/null and b/demos/selective_dualarm_grasping/test_data/04.jpg differ diff --git a/demos/selective_dualarm_grasping/test_data/05.jpg b/demos/selective_dualarm_grasping/test_data/05.jpg new file mode 100644 index 000000000..c815b5880 Binary files /dev/null and b/demos/selective_dualarm_grasping/test_data/05.jpg differ diff --git a/demos/selective_dualarm_grasping/test_data/06.jpg b/demos/selective_dualarm_grasping/test_data/06.jpg new file mode 100644 index 000000000..d6c7c488e Binary files /dev/null and b/demos/selective_dualarm_grasping/test_data/06.jpg differ diff --git a/demos/selective_dualarm_grasping/test_data/07.jpg b/demos/selective_dualarm_grasping/test_data/07.jpg new file mode 100644 index 000000000..e9e4291dc Binary files /dev/null and b/demos/selective_dualarm_grasping/test_data/07.jpg differ diff --git a/demos/selective_dualarm_grasping/test_data/08.jpg b/demos/selective_dualarm_grasping/test_data/08.jpg new file mode 100644 index 000000000..25c467ad2 Binary files /dev/null and b/demos/selective_dualarm_grasping/test_data/08.jpg differ diff --git a/demos/selective_dualarm_grasping/test_data/09.jpg b/demos/selective_dualarm_grasping/test_data/09.jpg new file mode 100644 index 000000000..30c98cbca Binary files /dev/null and b/demos/selective_dualarm_grasping/test_data/09.jpg differ diff --git a/demos/selective_dualarm_grasping/yaml/config.yaml b/demos/selective_dualarm_grasping/yaml/config.yaml new file mode 100644 index 000000000..51be89c3f --- /dev/null +++ b/demos/selective_dualarm_grasping/yaml/config.yaml @@ -0,0 +1,8 @@ +alpha: {dual: 1.0, single: 1.0} +dataset_class: DualarmGraspDatasetV2 +frequency_balancing: true +lr: 1.0e-05 +max_iter: 200000 +model: dualarm_grasp_fcn32s +random_state: 1234 +weight_decay: 0.0005 diff --git a/demos/selective_dualarm_grasping/yaml/dualarm_grasping_label_names.yaml b/demos/selective_dualarm_grasping/yaml/dualarm_grasping_label_names.yaml new file mode 100644 index 000000000..4b446b873 --- /dev/null +++ b/demos/selective_dualarm_grasping/yaml/dualarm_grasping_label_names.yaml @@ -0,0 +1,11 @@ +label_names: + - __background__ + - avery_binder + - composition_book + - hanes_socks + - ice_cube_tray + - reynolds_wrap + - robots_dvd + - scotch_sponges + - table_cloth + - toilet_brush diff --git a/demos/selective_dualarm_grasping/yaml/dualarm_occluded_grasping_label_names.yaml b/demos/selective_dualarm_grasping/yaml/dualarm_occluded_grasping_label_names.yaml new file mode 100644 index 000000000..cbd2e459c --- /dev/null +++ b/demos/selective_dualarm_grasping/yaml/dualarm_occluded_grasping_label_names.yaml @@ -0,0 +1,10 @@ +label_names: + - avery_binder + - composition_book + - hanes_socks + - ice_cube_tray + - reynolds_wrap + - robots_dvd + - scotch_sponges + - table_cloth + - toilet_brush diff --git a/demos/selective_dualarm_grasping/yaml/dualarm_occluded_grasping_target_names.yaml b/demos/selective_dualarm_grasping/yaml/dualarm_occluded_grasping_target_names.yaml new file mode 100644 index 000000000..e521bfa86 --- /dev/null +++ b/demos/selective_dualarm_grasping/yaml/dualarm_occluded_grasping_target_names.yaml @@ -0,0 +1,6 @@ +target_names: + - avery_binder + - composition_book + - hanes_socks + - reynolds_wrap + - toilet_brush diff --git a/demos/selective_dualarm_grasping/yaml/label_names.yaml b/demos/selective_dualarm_grasping/yaml/label_names.yaml new file mode 100644 index 000000000..6f23feb39 --- /dev/null +++ b/demos/selective_dualarm_grasping/yaml/label_names.yaml @@ -0,0 +1,42 @@ +label_names: + - __background__ + - avery_binder + - balloons + - band_aid_tape + - bath_sponge + - black_fashion_gloves + - burts_bees_baby_wipes + - colgate_toothbrush_4pk + - composition_book + - crayons + - duct_tape + - epsom_salts + - expo_eraser + - fiskars_scissors + - flashlight + - glue_sticks + - hand_weight + - hanes_socks + - hinged_ruled_index_cards + - ice_cube_tray + - irish_spring_soap + - laugh_out_loud_jokes + - marbles + - measuring_spoons + - mesh_cup + - mouse_traps + - pie_plates + - plastic_wine_glass + - poland_spring_water + - reynolds_wrap + - robots_dvd + - robots_everywhere + - scotch_sponges + - speed_stick + - table_cloth + - tennis_ball_container + - ticonderoga_pencils + - tissue_box + - toilet_brush + - white_facecloth + - windex