diff --git a/jsk_ik_server/README.md b/jsk_ik_server/README.md index ca19da8ac..f1d2c7005 100644 --- a/jsk_ik_server/README.md +++ b/jsk_ik_server/README.md @@ -46,6 +46,23 @@ For more detailed infomation. please read [docs/jaxon_ik_evaluation](docs/jaxon_ ![](./data/jaxon-zup-iterate009-concatenated.png) ![](./data/jaxon-zup-iterate009.csv.png) +### Fetch IK Evaluation + +#### X-up IK reachability with `:rotation-axis :z` + +![](./data/fetch-xup-iterate009-concatenated.png) +![](./data/fetch-xup-iterate009.csv.png) + +#### Y-up IK reachability with `:rotation-axis :z` + +![](./data/fetch-yup-iterate009-concatenated.png) +![](./data/fetch-yup-iterate009.csv.png) + +#### Z-up IK reachability with `:rotation-axis :z` + +![](./data/fetch-zup-iterate009-concatenated.png) +![](./data/fetch-zup-iterate009.csv.png) + ### Baxter IK Evaluation #### X-up IK reachability with `:rotation-axis :z` diff --git a/jsk_ik_server/data/fetch-xup-iterate009-concatenated.png b/jsk_ik_server/data/fetch-xup-iterate009-concatenated.png new file mode 100644 index 000000000..42a6b4a7a Binary files /dev/null and b/jsk_ik_server/data/fetch-xup-iterate009-concatenated.png differ diff --git a/jsk_ik_server/data/fetch-xup-iterate009.csv.png b/jsk_ik_server/data/fetch-xup-iterate009.csv.png new file mode 100644 index 000000000..71f1d31c5 Binary files /dev/null and b/jsk_ik_server/data/fetch-xup-iterate009.csv.png differ diff --git a/jsk_ik_server/data/fetch-yup-iterate009-concatenated.png b/jsk_ik_server/data/fetch-yup-iterate009-concatenated.png new file mode 100644 index 000000000..8c2feaa8b Binary files /dev/null and b/jsk_ik_server/data/fetch-yup-iterate009-concatenated.png differ diff --git a/jsk_ik_server/data/fetch-yup-iterate009.csv.png b/jsk_ik_server/data/fetch-yup-iterate009.csv.png new file mode 100644 index 000000000..f4404df3c Binary files /dev/null and b/jsk_ik_server/data/fetch-yup-iterate009.csv.png differ diff --git a/jsk_ik_server/data/fetch-zup-iterate009-concatenated.png b/jsk_ik_server/data/fetch-zup-iterate009-concatenated.png new file mode 100644 index 000000000..f272905ef Binary files /dev/null and b/jsk_ik_server/data/fetch-zup-iterate009-concatenated.png differ diff --git a/jsk_ik_server/data/fetch-zup-iterate009.csv.png b/jsk_ik_server/data/fetch-zup-iterate009.csv.png new file mode 100644 index 000000000..ad40b3c4a Binary files /dev/null and b/jsk_ik_server/data/fetch-zup-iterate009.csv.png differ diff --git a/jsk_ik_server/euslisp/ik-evaluation.l b/jsk_ik_server/euslisp/ik-evaluation.l index 975807269..fe91f7c40 100644 --- a/jsk_ik_server/euslisp/ik-evaluation.l +++ b/jsk_ik_server/euslisp/ik-evaluation.l @@ -517,12 +517,12 @@ ik-arg rot-matrix output-directory - &key (min-distance 0)) + &key (min-distance 0) (arm :larm)) (format t "Generate reachability map for ~A with rotation ~A~%" (send robot :name) rot-matrix) (setq *grid* (instance ik-grid :init min-max-x min-max-y min-max-z :grid-step grid-step)) - (ik-evaluation *robot* *grid* :larm ik-arg :update-view nil + (ik-evaluation *robot* *grid* arm ik-arg :update-view nil :rotation rot-matrix :min-distance min-distance) (send *grid* :dump-to-csv (format nil "~A/~A-input.csv" output-directory file-prefix)) diff --git a/jsk_ik_server/scripts/ik-grid/fetch.l b/jsk_ik_server/scripts/ik-grid/fetch.l new file mode 100755 index 000000000..60bc2b6e4 --- /dev/null +++ b/jsk_ik_server/scripts/ik-grid/fetch.l @@ -0,0 +1,36 @@ +#!/usr/bin/env roseus + +(load "package://jsk_ik_server/euslisp/ik-evaluation.l") + +(warning-message 2 "It takes much time! Be patient!~%") + +(require "package://fetcheus/fetch-interface.l") +(setq *robot* (make-robot-model-from-name "fetch")) +(setq *xrange* '(0.0 . 1000.0)) +(setq *yrange* '(-2000.0 . 2000.0)) +(setq *zrange* '(0.0 . 2000.0)) +(setq *output-directory* (ros::resolve-ros-path "package://jsk_ik_server/data/")) + +(generate-ik-grid-for-robot *robot* "fetch-zup" + *xrange* *yrange* *zrange* + 100 + '(:rotation-axis :z :use-torso t) + (unit-matrix) + *output-directory* + :arm :rarm) +(generate-ik-grid-for-robot *robot* "fetch-yup" + *xrange* *yrange* *zrange* + 100 + '(:rotation-axis :z :use-torso t) + (send (make-coords :rpy (list 0 0 pi/2)) :worldrot) + *output-directory* + :arm :rarm) +(generate-ik-grid-for-robot *robot* "fetch-xup" + *xrange* *yrange* *zrange* + 100 + '(:rotation-axis :z :use-torso t) + (send (make-coords :rpy (list 0 -pi/2 0)) :worldrot) + *output-directory* + :arm :rarm) + +(sys::exit 0) diff --git a/jsk_ik_server/scripts/ik-grid/fetch.sh b/jsk_ik_server/scripts/ik-grid/fetch.sh new file mode 100755 index 000000000..9c7ae4e99 --- /dev/null +++ b/jsk_ik_server/scripts/ik-grid/fetch.sh @@ -0,0 +1,15 @@ + #!/bin/sh + +rosrun jsk_ik_server fetch.l +find $(rospack find jsk_ik_server) -name fetch-*.csv -exec rosrun jsk_ik_server plot_ik_grid.py {} {}.png \; + +# Concatenate pickview images +for prefix in xup yup zup +do + convert +append ../../data/fetch-${prefix}-input-slant.png ../../data/fetch-${prefix}-input-up.png ../../data/fetch-${prefix}-input-side.png ../../data/fetch-${prefix}-input-front.png ../../data/fetch-${prefix}-input-concatenated.png + + for i in $(seq 0 9) + do + convert +append ../../data/fetch-${prefix}-iterate$(printf "%03d" ${i})-slant.png ../../data/fetch-${prefix}-iterate$(printf "%03d" ${i})-up.png ../../data/fetch-${prefix}-iterate$(printf "%03d" ${i})-side.png ../../data/fetch-${prefix}-iterate$(printf "%03d" ${i})-front.png ../../data/fetch-${prefix}-iterate$(printf "%03d" ${i})-concatenated.png + done +done